Diff: main.cpp

Revision:

7:1d57463393c6

Parent:

6:64146e16e10c

Child:

8:c7d3b67346db

--- a/main.cpp	Fri Oct 11 10:48:24 2019 +0000
+++ b/main.cpp	Fri Oct 11 12:31:34 2019 +0000
@@ -70,7 +70,7 @@
     }
 
 // Finite state machine programming (calibration servo motor?)
-enum states {Motors_off, Calib_motor, Calib_EMG, Homing, No_mov_before_pick_up, Servo_horizontal, Position_1, Slide, Lift, No_mov_before_drop_off, Position_drop_off, Servo_tilt_down, Position_2};
+enum states {Motors_off, Calib_motor, Calib_EMG, Homing, Operation_mode, Failure_mode};
 
 states currentState = Motors_off; 
 bool stateChanged = true; // Make sure the initialization of first state is executed
@@ -88,12 +88,19 @@
                 stateChanged = false;
                 pc.printf("Motors off\n");
             }            
-            if // (Motor_calib_button_pressed()) nog een extra button toevoegen voordat dit kan
+            if // (Power_button_pressed()) nog een button toevoegen voordat dit kan
             {      
                 currentState = Calib_motor;
                 stateChanged = true;
                 pc.printf("Moving to Calib_motor state\n");
             }
+            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
+            {
+                currentState = Failure_mode;
+                stateChanged = true;
+                pc.printf("Moving to failure_mode\n");
+            }
+                
             break;
             
         case Calib_motor:
@@ -104,7 +111,14 @@
                 currentState = Calib_EMG;
                 stateChanged = true;
                 pc.printf("Moving to Calib_EMG state\n");
-            }       
+            }   
+            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
+            {
+                currentState = Failure_mode;
+                stateChanged = true;
+                pc.printf("Moving to failure_mode\n");
+            }
+                
             break;
             
         case Calib_EMG:
@@ -116,78 +130,74 @@
                 stateChanged = true;
                 pc.printf("Moving to Homing state\n");
             }
+            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
+            {
+                currentState = Failure_mode;
+                stateChanged = true;
+                pc.printf("Moving to failure_mode\n");
+            }
+            
             break;
             
         case Homing:
             
             if (stateChanged)
             {   
-                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
-                currentState = No_mov_before_pick_up;
+                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm 
+                // (inclusief de end-effector) in de juiste home positie wordt gezet
+                currentState = Operation_mode;
+                stateChanged = true;
+                pc.printf("Moving to operation mode\n");
+            }
+            if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
+            {
+                currentState = Failure_mode;
                 stateChanged = true;
-                pc.printf("Moving to no movement before pick up state \n");
+                pc.printf("Moving to failure_mode\n");
+            }   
+            break;
+                  
+        case Operation_mode:
+        
+            if (stateChanged)
+            { 
+                // Hier moet een functie worden aangeroepen die ervoor zorgt dat 
+                // aan de hand van EMG-signalen de motoren kunnen worden aangestuurd, 
+                // zodat de robotarm kan bewegen   
+                
+                if // (Power_button_pressed())
+                {
+                    currentState = Motors_off;
+                    stateChanged = true;
+                }
+                if // (Emergency_button_pressed()) nog een button toevoegen voordat dit kan
+                {
+                    currentState = Failure_mode;
+                    stateChanged = true;
+                    pc.printf("Moving to failure_mode\n");     
+                }
+                else 
+                { 
+                    currentState = Homing; 
+                    stateChanged = true; 
+                }
+                break;
+                
+        case Failure_mode:
+        
+            if (stateChanged)
+            { 
+                pc.printf("Ik ga exploderen!!!");
+                // Alles moet uitgaan (evt. een rood LEDje laten branden), moet 
+                // opnieuw worden opgestart. Mogelijk kan dit door de ticker de 
+                // detachen
             }
             break;
             
-        case No_mov_before_pick_up: // De robot arm beweegt niet, soort ruststand na de homing
-        
-            if (stateChanged)
-            {
-                if (// eis om naar positie 1 te gaan)
-                {
-                currentState = Position_1
-                stateChanged = true;
-                pc.printf("Moving to position_1 state\n");
-                }
-                
-                if (// eis om naar positie 2 te gaan)
-                {
-                currentState = Position_2;
-                stateChanged = true;
-                pc.printf("Moving to position_2 state\n");
-                }
-                
-                if (// eis om naar drop off positie te gaan, als het eraf glijden mislukt is)
-                {
-                currentState = Position_drop_off;
-                stateChanged = true;
-                pc.printf("Moving to position_drop_off state\n");
-                }
-                
-                if (// eis voor nieuwe poging om home positie juist te bereiken)
-                {
-                currentState = Homing; // Mogelijk de state Calib_motor
-                stateChanged = true;
-                pc.printf("Retry homing\n");
-                }
-                
-                if (// eis om de motoren uit te zetten)
-                {
-                currentState = Motors_off
-                stateChanged = true;
-                pc.printf("Shutting down\n");
-                }
-            }
-            break;
-            
-        case Position_1:
-        
-            if (stateChanged)
-            { 
-                // Ervoor zorgen dat de end-effector 
-             
-        
-    
-           if (stateChanged)
-            {   
-                // Ervoor zorgen dat de motoren zo bewegen dat de robotarm in de juiste home positie wordt gezet
-                currentState = No_mov_before_pick_up;
-                stateChanged = true;
-                pc.printf("Moving to no movement before pick up state \n");
-            }
-            break;
-             
-                
+        default:
+            // Zelfde functie als die eerder is toegepast om motoren uit te schakelen -> safety!
+            pc.printf("Unknown or uninplemented state reached!\n");
+        }
 
 int main()
     {