Diff: main.cpp

Revision:

2:afa78538ef33

Parent:

1:1541f49d8680

diff -r 1541f49d8680 -r afa78538ef33 main.cpp
--- a/main.cpp	Fri Nov 02 11:44:30 2018 +0000
+++ b/main.cpp	Fri Nov 02 14:01:29 2018 +0000
@@ -593,22 +593,31 @@
         case CHOOSE:                            //State to choose between RKI and DC
             if(StateChanged)
             {
-                //pc.printf("Entering Basic Control Mode\r\n");
                 ledr = 1;
                 ledg = 1;
                 ledb = 0;
                 StateChanged = false;
             }
+            
+            while(button1 && button3 && button4)
+            {;}
+            
             if(!button3)
-            {
-                CurrentState = RKI;
-                StateChanged = true;
-            }
+                {
+                    CurrentState = RKI;
+                    StateChanged = true;
+                }
             else if(!button4)
-            {
-                CurrentState = DC;
-                StateChanged = true;
-            }
+                {
+                    CurrentState = DC;
+                    StateChanged = true;
+                }
+            else
+                {
+                    CurrentState = WAITING;
+                    StateChanged = true;
+                }
+            
             break;                              //End of "choose" state, move on to RKI or DC    (straight lines or direct control)
 
         case RKI:                               //State for moving the robot in straight lines in cartesian space
@@ -617,7 +626,7 @@
             {
                 //pc.printf("Entering Basic Control Mode\r\n");
                 ledr = 1;
-                ledg = 1;
+                ledg = 0;
                 ledb = 0;
                 StateChanged = false;
             }
@@ -658,12 +667,12 @@
                 start_v_des_calculate_qref = 0;
                 StateChanged = true;
             }
-            break; //End of BCM state
+            break; //End of RKI state
             
         case DC:
             if(StateChanged)
             {
-                ledr = 1;
+                ledr = 0;
                 ledg = 1;
                 ledb = 0;
                 StateChanged = false;
@@ -706,8 +715,8 @@
         case HOMING:                                            //Back to encoder position zero
             if(StateChanged)
             {
-                ledr = 1;
-                ledg = 1;
+                ledr = 0;
+                ledg = 0;
                 ledb = 0;
                 StateChanged = false;
             }
@@ -715,7 +724,38 @@
             while(button1 && !failure &&!backRKI &&!backDC)
             {
                 //Back to encoder position zero function  
+        
+                //translation back
+                while(encoder1.getPulses()>10.0 && encoder1.getPulses()<-10.0)
+                {
+                    
+                    if(encoder1.getPulses()>10.0)
+                    {
+                        pwmpin1 = 0.3f;
+                        directionpin1.write(0);
+                    }
+                    else if (encoder1.getPulses()<-10.0)
+                    {
+                        pwmpin1 = 0.3f;
+                        directionpin1.write(1);
+                    }
+                }
                 
+                //Rotation back
+                while(encoder2.getPulses()>10.0 && encoder2.getPulses()<-10.0)
+                {
+                    
+                    if(encoder2.getPulses()>10.0)
+                    {
+                        pmwpin2 = 0.3f;
+                        directionpin2.write(1);
+                    }
+                    else if (encoder2.getPulses()<-10.0)
+                    {
+                        pwmpin2 = 0.3f;
+                        directionpin2.write(0);
+                    }
+                }
                 
                  
                 if(!button3)
@@ -762,12 +802,18 @@
             {
                 failure = false;
                 StateChanged = false;
+                
+                ledr=0;
+                ledg=0;
+                ledb=0;
             }
             //State actions
             //"Fail mode is active. Press button1 to enter WAITING
-            while(button1)
-            {
-            }
+             while(button1)
+                {
+                    ledr = !ledr;
+                    wait(0.3f);
+                }
             //State transition
             CurrentState = WAITING;
             StateChanged = true;
@@ -780,7 +826,7 @@
             ledb = 0;
             while(button1)
             {
-                ledr = !ledr;
+                ledb = !ledb;
                 wait(0.3f);
             }
             CurrentState = WAITING;