Diff: main.cpp

Revision:

1:4cb9af313c26

Parent:

0:2a5dd6cc0008

Child:

2:5cace74299e7

diff -r 2a5dd6cc0008 -r 4cb9af313c26 main.cpp
--- a/main.cpp	Tue Oct 23 13:04:18 2018 +0000
+++ b/main.cpp	Fri Oct 26 09:12:26 2018 +0000
@@ -9,12 +9,14 @@
 
 DigitalIn startButton(D0);
 InterruptIn failureButton(D1);
-DigitalIn grippingSwitch(D2);
-DigitalIn screwingSwitch(D3);
+DigitalIn grippingSwitch(SW2);
+DigitalIn screwingSwitch(SW3);
+DigitalIn gripDirection(D2);
+DigitalIn screwDirection(D3);
 MODSERIAL pc(USBTX, USBRX);
-DigitalOut led1(LED1);
-DigitalOut led2(LED2);
-DigitalOut led3(LED3);
+DigitalOut led1(LED1); // Red led
+DigitalOut led2(LED2); // Green led
+DigitalOut led3(LED3); // Blue led
 
 bool stateChanged = true;
 
@@ -25,7 +27,10 @@
 void switchToFailureState ()
 {
     failureButton.fall(NULL); // Detaches button, so it can only be activated once
-    pc.printf("SYSTEM FAILED");
+    led1 = 0;
+    led2 = 1;
+    led3 = 1;
+    pc.printf("SYSTEM FAILED\n");
     currentState = FailureState;
     stateChanged = true;
 }
@@ -35,14 +40,17 @@
     switch (currentState) {
         case WaitState:
             if (stateChanged == true) {
+                led1 = 0;
+                led2 = 1;
+                led3 = 1;
                 // 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;
+            if (startButton.read() == false) {
+                //pc.printf("Switching to motor calibration");
+                led1 = 1;
                 currentState = MotorCalState;
                 stateChanged = true;
             }
@@ -51,7 +59,7 @@
         case MotorCalState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state
-                led2 = 1;
+                led2 = 0;
                 // Set motorpwm to 'low' value
                 stateTimer.reset();
                 stateTimer.start();
@@ -61,8 +69,7 @@
             // Add stuff you do every loop
 
             if (stateTimer >= 3.0f) { //TODO: add && motor velocity < 0.1f
-                pc.printf("Starting EMG calibration...\n");
-                led2 = 0;
+                led2 = 1;
                 currentState = EMGCalState;
                 stateChanged = true;
             }
@@ -70,7 +77,7 @@
         case EMGCalState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
-                led3 = 1;
+                led3 = 0;
                 stateTimer.reset();
                 stateTimer.start();
                 stateChanged = false;
@@ -79,8 +86,8 @@
             // Add stuff you do every loop
 
             if (stateTimer >= 3.0f) {
-                pc.printf("Starting homing...\n");
-                led3 = 0;
+                //pc.printf("Starting homing...\n");
+                led3 = 1;
                 currentState = HomingState;
                 stateChanged = true;
             }
@@ -88,43 +95,102 @@
         case HomingState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
-                led1 = 1;
-                led2 = 1;
-                // Set intended position
+                led1 = 0;
+                led2 = 0; // Emits yellow together
+                //TODO: 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;
+            if (startButton.read() == false) { //TODO: Also add position condition
+                led1 = 1;
+                led2 = 1;
                 currentState = MovingState;
                 stateChanged = true;
             }
             break;
         case MovingState:
-        if (stateChanged == true) {
+            if (stateChanged == true) {
                 // Entry action: all the things you do once in this state;
+                led1 = 0;
+                led2 = 0;
+                led3 = 0; // Emits white together
+                stateChanged = false;
+            }
+
+            if (grippingSwitch.read() == false) {
                 led1 = 1;
                 led2 = 1;
                 led3 = 1;
-                // Set intended position
+                currentState = GrippingState;
+                stateChanged = true;
+            }
+
+            break;
+        case GrippingState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state;
+                led2 = 0;
+                led3 = 0; // Emits x together
                 stateChanged = false;
             }
+
+            if (gripDirection == true) {
+                // Close gripper
+            } else {
+                // Open gripper
+            }
+
+            if (screwingSwitch.read() == false) {
+                led2 = 1;
+                led3 = 1;
+                currentState = ScrewingState;
+                stateChanged = true;
+            }
+            if (startButton.read() == false) {
+                led2 = 1;
+                led3 = 1;
+                currentState = MovingState;
+                stateChanged = true;
+            }
             break;
-        case GrippingState:
-            break;
         case ScrewingState:
+            if (stateChanged == true) {
+                // Entry action: all the things you do once in this state;
+                led1 = 0;
+                led3 = 0; // Emits pink together
+                stateChanged = false;
+            }
+            
+            if (screwDirection == true) {
+                // Screw
+            } else {
+                // Unscrew
+            }
+            
+            if (startButton.read() == false) {
+                led1 = 1;
+                led3 = 1;
+                currentState = MovingState;
+                stateChanged = true;
+            }
             break;
         case FailureState:
             if (stateChanged == true) {
                 // Entry action: all the things you do once in this state
-                // Turn all motors off
+                //TODO: Turn all motors off
                 stateChanged = false;
-                break;
             }
+
+            static double blinkTimer = 0;
+            if (blinkTimer >= 0.5) {
+                led1 = !led1;
+                blinkTimer = 0;
+            }
+            blinkTimer += sampleTime;
+
+            break;
     }
 }