Diff: main.cpp

Revision:

12:f5dc65f1c27b

Parent:

11:a3fd9d5144bb

Child:

13:db1a8b51706b

--- a/main.cpp	Tue Oct 29 13:52:23 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:56:19 2019 +0000
@@ -24,8 +24,8 @@
 //*****************************************************************************
 // 2. States ******************************************************************
 //*****************************************************************************
-enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Operating, Emergency, Demo}; //All robot states
-States state;
+enum States {StartWait, MotorCalibration, EMGCalibration, Homing, Demo, Operating, EmergencyMode, Idle}; //All robot states
+States State;
 
 //*****************************************************************************
 // 3. (Global) Variables ***********************************************************
@@ -174,13 +174,7 @@
         motor2=0.0;
         motor2_calibrated=true;  
         pc.printf("\r\n Motor2 kalibratie = %s", motor2_calibrated ? "true" : "false");
-        
-        if (motor1_calibrated==true&&motor2_calibrated==true) 
-        {
-            pc.printf("\r\n Motor Calibration is done!");
-        }
-        else {pc.printf("\r\n Motor Calibration is not done!");}
-    
+            
     }
     
     // 4.2a PID-Controller motor 1**************************************************
@@ -233,6 +227,92 @@
     
     // 4.3 State-Machine *******************************************************
 
+    void state_machine()
+{
+    if (sw2==0) {State = EmergencyMode;}
+    switch(State)
+    {       
+        case MotorCalibration: 
+//            pc.printf("\r\n State: MotorCalibration");
+            led_blue.write(1);
+            led_red.write(1);
+            led_green.write(0); //Green Led on when in this state
+            
+            if (motor1_calibrated==true&&motor2_calibrated==true) 
+            {
+                pc.printf("\r\n Motor Calibration is done!");
+                State=StartWait; 
+            }
+            else {;} //pc.printf("\r\n Motor Calibration is not done!");}
+            
+            
+            break; 
+                
+        case StartWait:  
+ //           pc.printf("\r\n State: StartWait Button 1 = operation, Button 2 = Demo"); 
+            led_blue.write(0);
+            led_red.write(1);
+            led_green.write(1);
+           
+            if(button1==0) {State=EMGCalibration;}
+            if(button2==0) {State=Demo;} 
+            break;             
+        case EMGCalibration: 
+   //         pc.printf("\r\n State: EMGCalibration");
+            led_blue.write(1);
+            led_red.write(1);
+            led_green.write(0);
+
+            State=Homing; 
+            break; 
+        case Homing: 
+  /*          pc.printf("\r\n State: Homing");
+            State=Operating; */
+            break; 
+        case Demo:
+           pc.printf("\r\n State: Demo");
+            led_blue.write(1);
+            led_red.write(1);
+            led_green.write(0); //!=led_green.write(0);
+
+            
+            break;  
+        case Operating:
+  /*          pc.printf("\r\n State: Operating");
+            led_blue.write(1);
+            led_red.write(1);
+            led_green.write(0);
+            wait(0.5);
+            led_green.write(1);
+            wait(0.5); */
+            break; 
+        case EmergencyMode:
+            pc.printf("\r\n State: EMERGENCY MODE! Press RESET to restart");
+            
+            motor1=0; 
+            motor2=0;
+            
+            led_blue.write(1);
+            led_green.write(1);
+            //SOS start
+            led_red.write(0); // S
+            wait(0.5);
+            led_red.write(1); //pause
+            wait(0.25);
+            led_red.write(0); // O
+            wait(1.5);
+            led_red.write(1); // pause
+            wait(0.25);
+            led_red.write(0); // S
+            wait(0.5);
+            //SOS end
+            break; 
+        case Idle:
+   /*         pc.printf("\r\n Idling..."); */
+            break;
+
+        }
+}
     
 //******************************************************************************
 // 5. Main Loop ****************************************************************
@@ -242,7 +322,7 @@
 //    pc.printf("main_loop is running succesfully \r\n"); //confirmation that main_loop is running (als je dit erin zet krijg je elke duizendste dit bericht. Dit is niet gewenst)
 fencoder_motor1() ;
 fencoder_motor2() ;
-
+state_machine() ;
 
 // 5.1 Measure Analog and Digital input signals ********************************
 // 5.2 Run state-machine(s) ****************************************************
@@ -269,7 +349,7 @@
     led_green.write(1);
     led_red.write(1);
     led_blue.write(0);
-   
+    State = MotorCalibration;
     ticker_mainloop.attach(&main_loop,0.001); // change back to 0.001f //Run the function main_loop 1000 times per second       
     ticker_hidscope.attach(&HIDScope, 0.001); //Ticker for Hidscope, different frequency compared to motors
     motor_calibration();