Diff: main.cpp

Revision:

3:dca57056e5cb

Parent:

2:b6b962d7db36

Child:

4:a0c1c021026b

--- a/main.cpp	Mon Oct 22 14:19:02 2018 +0000
+++ b/main.cpp	Mon Oct 22 14:45:37 2018 +0000
@@ -1,10 +1,144 @@
 #include "mbed.h"
+#include "MODSERIAL.h"
 
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalOut led_blue(LED_BLUE);
+DigitalIn button(SW2);
+InterruptIn Fail_button(SW3);
+MODSERIAL pc(USBTX, USBRX);
+
+enum states {WAITING,MOTOR_ANGLE_CLBRT,EMG_CLBRT,HOMING,WAITING4SIGNAL,MOVE_W_EMG,MOVE_W_DEMO,FAILURE_MODE,SHUTDOWN};
+states currentState = WAITING;
+char c;
+    
+void Failing() {
+    currentState = FAILURE_MODE;
+    wait(1.0f);
+    }
 
 int main()
 {
+    led_red = 1;
+    led_green = 1;
+    led_blue = 1;
+    pc.baud(115200);
+    Fail_button.fall(&Failing);    
+
+    pc.printf("\r\n______________ STATE MACHINE ______________ \r\n\r\n");
+    
     while (true) {
-        //dit is een test - kusjes silvie
-        // Nog meer test! jeej woehoe
+        switch (currentState)
+        {
+            case WAITING:
+                pc.printf("WAITING... \r\n");
+                led_red = 0; led_green = 0; led_blue = 0;   // Colouring the led WHITE
+                if (!button) {
+                    currentState = MOTOR_ANGLE_CLBRT;
+                    }
+                wait(1.0f);
+                break;
+            case MOTOR_ANGLE_CLBRT:
+                pc.printf("Motor angle calibration \r\n");
+                led_red = 1; led_green = 0; led_blue = 0;   // Colouring the led TURQUOISE
+                wait(3.0f); // time_in_this_state > 3.0f
+                // if (fabs(motor_velocity) < 0.01f) {
+                currentState = EMG_CLBRT;
+                break;
+            case EMG_CLBRT:
+                pc.printf("EMG calibration \r\n");
+                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led BLUE
+                wait(5.0f); // time_in_this_state > 5.0f
+                // if moving_average(procd_emg) < 0.1*max_procd_emg_ever
+                currentState = HOMING;
+                break;                   
+            case HOMING:
+                pc.printf("HOMING, moving to the home starting configuration. \r\n");
+                led_red = 0; led_green = 1; led_red = 0;                  // Colouring the led PURPLE
+                wait(5.0f); // time_in_this_state > 5.0f
+                // if ((fabs(angle_error1) < 0.01f) && (fabs(angle_error2) < 0.01f)) {
+                currentState = WAITING4SIGNAL;
+                break;
+            case WAITING4SIGNAL:
+                led_red = 0; led_green = 0; led_blue = 0;                // Colouring the led WHITE
+                pc.printf("Press d to run the demo, Press e to move with EMG, Press c to re-calibrate or Press s to shut the robot down. \r\n");
+                c = pc.getc();
+                if (c == 'd') {
+                    currentState = MOVE_W_DEMO;
+                    }
+                else if (c == 'e') {
+                    currentState = MOVE_W_EMG;
+                    }
+                else if (c == 'c') {
+                    currentState = MOTOR_ANGLE_CLBRT;
+                    }
+                else if (c == 's') {
+                    currentState = SHUTDOWN;
+                    }
+                break;
+            case MOVE_W_DEMO:
+                led_red = 1; led_green = 1; led_blue = 0;   // Colouring the led GREEN
+                pc.printf("MOVE_W_DEMO, Running the demo \r\n");
+                c = pc.getcNb();
+                if (c == 'h') {
+                    currentState = HOMING;
+                    }
+                Fail_button.fall(&Failing); 
+                wait(1.0f);
+                break;
+            case MOVE_W_EMG:
+                led_red = 1; led_green = 0; led_blue = 1;   // Colouring the led GREEN
+                pc.printf("MOVE_W_EMG, Moving with use of EMG \r\n");
+                wait(1);
+                c = pc.getcNb();
+                if (c == 'h') {
+                    currentState = HOMING;
+                    }
+                Fail_button.fall(&Failing); 
+                wait(1.0f);
+                break;       
+            case FAILURE_MODE:
+                led_red = 0; led_green = 1; led_blue = 1;   // Colouring the led RED
+                pc.printf("FAILURE MODE \r\n");
+                wait(2.0f);
+                break;
+            case SHUTDOWN:
+                pc.printf("Shutting down \r\n");
+                led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
+                wait(1.0f);
+                for (int i = 0; i < 4; i++) {               // Blinking WHITE 3x before turning BLACK
+                    led_red = !led_red;
+                    led_green = !led_green;
+                    led_blue = !led_blue;
+                    wait(0.6f);
+                    }
+                    led_red = !led_red;
+                    led_green = !led_green;
+                    led_blue = !led_blue;
+                    wait(1.0f);
+                    led_red = !led_red;
+                    led_green = !led_green;
+                    led_blue = !led_blue;
+                while (true) {
+                    }
+
+                        
+            default: 
+                pc.printf("Unknown or unimplemented state reached!!! \n\r");
+                led_red = 1; led_green = 1; led_blue = 1;   // Colouring the led BLACK
+                for (int n = 0; n < 50; n++) {              // Making an SOS signal with the RED led
+                    for (int i = 0; i < 6; i++) { 
+                        led_red = !led_red;
+                        wait(0.6f);
+                        }
+                    wait(0.4f);
+                    for (int i = 0 ; i < 6; i++) {
+                        led_red = !led_red;
+                        wait(0.2f);
+                        }
+                    wait(0.4f);
+                }
+        }   // end of switch
     }
-}
\ No newline at end of file
+}   
+