Diff: main.cpp

Revision:

0:2a5dd6cc0008

Child:

1:4cb9af313c26

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 23 13:04:18 2018 +0000
@@ -0,0 +1,147 @@
+#include "mbed.h"
+#include "FastPWM.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "BiQuad.h"
+
+enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState};
+States currentState = WaitState;
+
+DigitalIn startButton(D0);
+InterruptIn failureButton(D1);
+DigitalIn grippingSwitch(D2);
+DigitalIn screwingSwitch(D3);
+MODSERIAL pc(USBTX, USBRX);
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+
+bool stateChanged = true;
+
+Ticker mainTicker;
+Timer stateTimer;
+double sampleTime = 0.01;
+
+void switchToFailureState ()
+{
+    failureButton.fall(NULL); // Detaches button, so it can only be activated once
+    pc.printf("SYSTEM FAILED");
+    currentState = FailureState;
+    stateChanged = true;
+}
+
+void stateMachine ()
+{
+    switch (currentState) {
+        case WaitState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state
+                // Set output motor to 0
+                stateChanged = false;
+            }
+
+            if (startButton.read() == true) {
+                pc.printf("Switching to motor calibration");
+                led1 = 0;
+                currentState = MotorCalState;
+                stateChanged = true;
+            }
+
+            break;
+        case MotorCalState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state
+                led2 = 1;
+                // Set motorpwm to 'low' value
+                stateTimer.reset();
+                stateTimer.start();
+                stateChanged = false;
+            }
+
+            // Add stuff you do every loop
+
+            if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
+                pc.printf("Starting EMG calibration...\n");
+                led2 = 0;
+                currentState = EMGCalState;
+                stateChanged = true;
+            }
+            break;
+        case EMGCalState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state;
+                led3 = 1;
+                stateTimer.reset();
+                stateTimer.start();
+                stateChanged = false;
+            }
+
+            // Add stuff you do every loop
+
+            if (stateTimer >= 3.0f) {
+                pc.printf("Starting homing...\n");
+                led3 = 0;
+                currentState = HomingState;
+                stateChanged = true;
+            }
+            break;
+        case HomingState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state;
+                led1 = 1;
+                led2 = 1;
+                // Set intended position
+                stateChanged = false;
+            }
+
+            // Nothing extra happens till robot reaches starting position and button is pressed
+
+            if (startButton.read() == true) { // Also add position condition
+                pc.printf("Start moving...\n");
+                led1 = 0;
+                led2 = 0;
+                currentState = MovingState;
+                stateChanged = true;
+            }
+            break;
+        case MovingState:
+        if (stateChanged == true) {
+                // Entry action: all the things you do once in this state;
+                led1 = 1;
+                led2 = 1;
+                led3 = 1;
+                // Set intended position
+                stateChanged = false;
+            }
+            break;
+        case GrippingState:
+            break;
+        case ScrewingState:
+            break;
+        case FailureState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state
+                // Turn all motors off
+                stateChanged = false;
+                break;
+            }
+    }
+}
+
+void mainLoop ()
+{
+    // Add measure, motor controller and output function
+    stateMachine();
+}
+
+int main()
+{
+    pc.baud(115200);
+    mainTicker.attach(mainLoop, sampleTime);
+    failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated
+
+    while (true) {
+        pc.printf("State = %i\n", currentState);
+        wait(1);
+    }
+}
\ No newline at end of file