Diff: Opzet_Eli.cpp

Revision:

1:268bf7dbb15c

Parent:

0:50f25a675c72

Child:

2:5730195cf595

--- a/Opzet_Eli.cpp	Mon Oct 07 07:51:23 2019 +0000
+++ b/Opzet_Eli.cpp	Mon Oct 07 08:26:51 2019 +0000
@@ -1,5 +1,6 @@
 // Robot states
 #include "mbed.h"
+#include "HIDScope.h"
 #include "MODSERIAL.h"
 
 // Define objects
@@ -9,26 +10,26 @@
 DigitalIn ptc1(PTC6); //Button 1
 DigitalIn ptc2(PTA4); //Button 2
 
-//v_motor, t_passed, EMG zijn allemaal inputs die we nog even moeten fiksen ik weet niet hoe
 // Functions 
 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
 
-states CurrentState(START);
-bool StateChanged true;         // this is the initialization of the first state
+states CurrentState = START;
+bool StateChanged = true;         // this is the initialization of the first state
 
 void StateMachine(void)
 {
     switch(CurrentState)
     {
-        case START:
-            while(true)     //RED LED ON
+        case START:     
+            if (StateChanged)
             {
+                pc.printf("Start state, red led is on. If button 1 pressed, go to kal_me state");
+                led1 = 0;  // Red led is on 
                 led2 = 1;
                 led3 = 1;
-                led1 = !led1;   
+                StateChanged = false;
             }
-            
-            if (ptc1 == 0) // State switches when button pressed
+            if (ptc1 = 0) // State switches when button pressed
             {
                 CurrentState = KAL_ME;
                 StateChanged = true;
@@ -36,14 +37,24 @@
         break;      // end of state START
                 
         case KAL_ME:
-            while(true)
+            if (StateChanged)
             {
-                led2 = 1;
-                led3 = 1;
-                led1 = !led1;
-                wait(0.7);
+                pc.printf("Calibration ME state, red ld flickers slow. 
+                while(true)         // Red lef flickers slow
+                {
+                    led2 = 1;
+                    led3 = 1;
+                    led1 = !led1;
+                    wait(0.7);
+                }
+                
+                // FUNCTION Move to mechanical stop, include v_motor
+                // FUNCTION Reset encoders
+                
+                StateChanged = false;
             }
-            if  (v_motor == 0 && t_passed = 2) //sec
+            
+            if  (v_motor == 0; t_passed > 2) // FUNCTION t_passed, included in v_motor?
             {
                 CurrentState = KAL_EMG;
                 StateChanged = true;
@@ -51,20 +62,21 @@
         break;      // end of state KAL_ME
         
         case KAL_EMG:
-            while(true)
+            if (StateChanged)
             {
-                led2 = 1;
-                led3 = 1;
-                led1 = !led1;
-                wait(0.2);
+                while(true)     // Red led flickers fast
+                {
+                    led2 = 1;
+                    led3 = 1;
+                    led1 = !led1;
+                    wait(0.2);
+                }
+                //FUNCTION Measure EMG_max
+                
+                StateChanged = false;
             }
             
-            while(t_passed < 5) //sec
-            {
-                //EMG_max = max EMG measured in these 5 sec
-            }
-            
-            if (EMG <= 0.1*EMG_max && t_passed == 2) //sec
+            if (EMG < 0.1*EMG_max && t_passed > 2)
             {
                 CurrentState = MOVE_START;
                 StateChanged = true;
@@ -72,17 +84,15 @@
         break;      // end of state KAL_EMG
         
         case MOVE_START:
-            while(true)
+            if(StateChanged)
             {
                 led1 = 1;
-                led2 = 1;
-                led3 = !led3;
-                wait(0.2);
-            }
+                led2 = 1; 
+                led3 = 0;   // Green led is on
             
             // define start position
             
-            if (t_passed == 2 && //position realised and tmie passed
+            if //position realised and tmie passed
             {
                 // Stop moving
                 CurrentState = READY_START;