Revision 21:041cc8aaac31, committed 2019-10-04

Comitter:

ehopman

Date:

Fri Oct 04 08:28:07 2019 +0000

Parent:

20:184839c241a7

Commit message:

Statemachine Eli 4-10-2019;

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Opzet_Eli_copy.cpp	Fri Oct 04 08:28:07 2019 +0000
@@ -0,0 +1,178 @@
+// Robot states
+#include "mbed.h"
+#include "HIDScope.h"
+#include "MODSERIAL.h"
+
+// Define objects
+DigitalOut led1(LED_RED);
+DigitalOut led2(LED_BLUE);
+DigitalOut led3(LED_GREEN);
+DigitalIn ptc1(PTC6); //Button 1
+DigitalIn ptc2(PTA4); //Button 2
+
+// 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
+
+void StateMachine(void)
+{
+    switch(CurrentState)
+    {
+        case START:
+            while(true)     //RED LED ON
+            {
+                led2 = 1;
+                led3 = 1;
+                led1 = !led1;   
+            }
+            if (ptc1 == 0) // State switches when button pressed
+           {
+                CurrentState = KAL_ME;
+                StateChanged = true;
+           }
+        break;      // end of state START
+                
+        case KAL_ME:
+            while(true)
+            {
+                led2 = 1;
+                led3 = 1;
+                led1 = !led1;
+                wait(0.7);
+            }
+            
+            while(motor != 1) // motor definen met een pin i guess
+            {
+                // move motors, geen idee nog hoe 
+            }
+            
+            if  //t = 2 and or motor stops moving
+            {
+                // Stop moving
+                CurrentState = KAL_EMG;
+                StateChanged = true;
+            }
+        break;      // end of state KAL_ME
+        
+        case KAL_EMG:
+            while(true)
+            {
+                led2 = 1;
+                led3 = 1;
+                led1 = !led1;
+                wait(0.2);
+            }
+            
+            while(EMG<=EMG_MAX)
+            {
+                //measure max emg
+            }
+            
+            if // emg <=10%max and t=2 time has passed
+            {
+                // Stop moving
+                CurrentState = MOVE_START;
+                StateChanged = true;
+            }
+        break;      // end of state KAL_EMG
+        
+        case MOVE_START:
+            while(true)
+            {
+                led1 = 1;
+                led2 = 1;
+                led3 = !led3;
+                wait(0.2);
+            }
+            
+            // define start position
+            
+            if //position realised and tmie passed
+            {
+                // Stop moving
+                CurrentState = READY_START;
+                StateChanged = true;
+            } 
+        break;      // end of state MOVE_START
+        
+        case READY_START:
+            while(true)
+            {
+                led1 = 1;
+                led2 = 1;
+                led3 = 0;
+            }
+            
+            if (ptc1 == 0) 
+            {
+                CurrentState = DEMO;
+                StateChanged = true;
+            }
+            else if (ptc2 == 0) // or EMG signal >= 20% max
+            {
+                CurrentState = MOVE;
+                StateChanged = true;
+            }                
+        break;      // end of state READY_START
+        
+        case DEMO:
+            //perform straight movements in some way, dunno how to make that here yet
+            if //position = end and time 
+            {
+                CurrentState = MOVE_START;
+                StateChanged = true;
+            }
+        break;      // end of state DEMO
+        
+        case MOVE:
+            if // no movement for 2 sec after input
+            {
+                CurrentState = WAIT;
+                StateChanged = true;
+            }
+            else if // input signal
+            {
+                // move in direction linked to EMG
+            }
+            // Iets met dat oranje lichtje maar snap die stap niet helemaal 
+        break;      // end of state MOVE
+        
+        case WAIT:
+            if (ptc1 == 0)
+            {
+                CurrentState = READY_START;
+                StateChanged = true;
+            )
+            else if (ptc2 == 0) // or EMG>= 20%max
+            {
+                CurrentState = MOVE;
+                StateChanged = true;
+            }
+            else if (ptc1 == 0) //maar dan heel lang achter elkaar
+        break;      // end of state WAIT
+        
+        case OFF:
+            // turns all of 
+        break;      // end of state OFF
+        
+        default:
+        TurnMotorsoff();
+        printf("Unknown state reached");
+    }               // End of the switch, all states are prescribed
+}
+
+int main(void)      // wat hier in moet snap ik nog niet 
+{
+        // hier moeten dingen komen
+        while (true)
+        {
+            CheckForCommandFromTerminal();
+            StateMachine();
+        }
+} 
+        
+        
+        
+