Diff: main.cpp

Revision:

9:e8cc37a94fec

Parent:

8:7dab565a208e

Child:

10:8c38a1a5b522

--- a/main.cpp	Mon Oct 14 14:00:10 2019 +0000
+++ b/main.cpp	Mon Oct 21 12:17:39 2019 +0000
@@ -9,17 +9,45 @@
 #include "Motor_Control.h"
 
 DigitalIn button1(D12);
-AnalogIn pot2(A0);
+InterruptIn button2(SW2);
+DigitalOut ledr(LED_RED);
+
+AnalogIn shield0(A0);
+AnalogIn shield1(A1);
+AnalogIn shield2(A2);
+AnalogIn shield3(A3);
+
 Ticker measurecontrol;
 DigitalOut motor1Direction(D7);
 FastPWM motor1Velocity(D6);
 MODSERIAL pc(USBTX, USBRX);
 QEI Encoder(D8,D9,NC,8400);
 
-float PI = 3.1415926535897932384626433832795028841971693993;
-float timeinterval = 0.001;
+float PI = 3.1415926;//535897932384626433832795028841971693993;
+float timeinterval = 0.001;         // Time interval of the Ticker function
+bool whileloop = true;              // Statement to keep the whileloop in the main function running
+                                    // While loop has to stop running when failuremode is activated
+
+// Define the different states in which the robot can be
+enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
+    START_GAME, DEMO_MODE, GAME_MODE, MOVE_END_EFFECTOR,
+    MOVE_GRIPPER, END_GAME, FAILURE_MODE};
+
+// Default state is the state in which the motors are turned off
+States MyState = MOTORS_OFF;
 
-// README
+
+
+void motorsoff() {
+    bool aa = true;                                 // Boolean for the while loop
+    sendtomotor(0);
+    while (aa) {
+        if (button1.read() == 0) {                  // If button1 is pressed, set MyState to EMG_CALIBRATION and exit the while loop
+            MyState = EMG_CALIBRATION;
+            aa = false;
+        }
+    }
+}
 
 
 //P control implementation (behaves like a spring)
@@ -36,20 +64,87 @@
 void nothing(){// Do nothing
 }
 
+
+void New_State() {
+    switch (MyState)
+    {
+        case MOTORS_OFF :
+            pc.printf("State: Motors turned off");
+            motorsoff();
+            break;
+        
+        case EMG_CALIBRATION :
+            pc.printf("State: EMG Calibration");
+            break;
+        
+        case MOTOR_CALIBRATION :
+            pc.printf("State: Motor Calibration");
+            break;
+        
+        case START_GAME :
+            pc.printf("State: Start game");
+            break;
+        
+        case DEMO_MODE :
+            pc.printf("State: Demo mode");
+            break;
+        
+        case GAME_MODE :
+            pc.printf("State: Game mode");
+            break;
+        
+        case MOVE_END_EFFECTOR :
+            pc.printf("State: Move end effector");
+            break;
+        
+        case MOVE_GRIPPER :
+            pc.printf("State: Move the gripper");
+            break;
+        
+        case END_GAME :
+            pc.printf("State: End of the game");
+            break;
+        
+        case FAILURE_MODE :
+            pc.printf("FAILURE MODE!!");            // Let the user know it is in failure mode
+            ledr = 0;                               // Turn red led on to show that failure mode is active
+            whileloop = false;
+            break;
+        
+        default :
+            pc.printf("Default state: Motors are turned off");
+            sendtomotor(0);
+            break;
+    }
+}
+
+void failuremode() {
+    MyState = FAILURE_MODE;
+    New_State();
+}
+
 int main()
 {
+    pc.printf("Starting...\r\n\r\n");
     double frequency = 17000.0;
     double period_signal = 1.0/frequency;
     pc.baud(115200);
-    pc.printf("Starting...\r\n\r\n");
+    
+    button2.fall(failuremode);          // Function is always activated when the button is pressed
     motor1Velocity.period(period_signal);
     measurecontrol.attach(measureandcontrol, timeinterval);
-    bool aa = true;
-    while(aa)
+    
+    int previous_state_int = (int)MyState;
+    New_State();
+    
+    while(whileloop)
     {
-        char c = pc.getcNb();
-        aa = c == 'c' ? false : true;
+        if ( (previous_state_int - (int)MyState) != 0 ) {
+            New_State();
+        }
+        previous_state_int = (int)MyState;
     }
+    
     pc.printf("Programm stops running\r\n");
     sendtomotor(0);
     measurecontrol.attach(nothing,10000);