Revision 2:5730195cf595, committed 2019-10-14

Comitter:

ehopman

Date:

Mon Oct 14 12:14:18 2019 +0000

Parent:

1:268bf7dbb15c

Commit message:

dok

Changed in this revision

--- a/Opzet_Eli.cpp	Mon Oct 07 08:26:51 2019 +0000
+++ b/Opzet_Eli.cpp	Mon Oct 14 12:14:18 2019 +0000
@@ -1,14 +1,17 @@
 // Robot states
 #include "mbed.h"
 #include "HIDScope.h"
+#include "QEI.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
+MODSERIAL pc(USBTX,USBRX);
+DigitalOut led_red(LED_RED);
+DigitalOut led_blue(LED_BLUE);
+DigitalOut led_green(LED_GREEN);
+InterruptIn button_Mbed(PTC6); //Button 1 Mbed
+InterruptIn button_1(PTB10); //Button 2 BRS
+InterruptIn button_2(PTB11); // Button 3 BRS
 
 // Functions 
 enum states{START,KAL_ME,KAL_EMG,MOVE_START,READY_START,DEMO,MOVE,WAIT,OFF};
@@ -16,6 +19,8 @@
 states CurrentState = START;
 bool StateChanged = true;         // this is the initialization of the first state
 
+// Function START_TO_KAL_ME
+
 void StateMachine(void)
 {
     switch(CurrentState)
@@ -24,37 +29,34 @@
             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;
+                led_red = 0;  // Red led is on 
+                led_blue = 1;
+                led_green = 1;
                 StateChanged = false;
             }
-            if (ptc1 = 0) // State switches when button pressed
+            if (button_Mbed.mode (PullDown)== false; ) // State switches when button pressed
             {
                 CurrentState = KAL_ME;
                 StateChanged = true;
+                wait(0.2f);
+                
             }
         break;      // end of state START
                 
         case KAL_ME:
             if (StateChanged)
             {
-                pc.printf("Calibration ME state, red ld flickers slow. 
-                while(true)         // Red lef flickers slow
-                {
-                    led2 = 1;
-                    led3 = 1;
-                    led1 = !led1;
-                    wait(0.7);
-                }
+                pc.printf("Calibration ME state, red ld flickers slow"); 
+                //FUNCTION Red led flickers slow
                 
-                // FUNCTION Move to mechanical stop, include v_motor
+                
+                // FUNCTION Move to mechanical stop, include v_motor, t_passed
                 // FUNCTION Reset encoders
                 
                 StateChanged = false;
             }
             
-            if  (v_motor == 0; t_passed > 2) // FUNCTION t_passed, included in v_motor?
+            if  (v_motor == 0 && t_passed > 2) // FUNCTION t_passed, included in v_motor?
             {
                 CurrentState = KAL_EMG;
                 StateChanged = true;
@@ -64,14 +66,9 @@
         case KAL_EMG:
             if (StateChanged)
             {
-                while(true)     // Red led flickers fast
-                {
-                    led2 = 1;
-                    led3 = 1;
-                    led1 = !led1;
-                    wait(0.2);
-                }
-                //FUNCTION Measure EMG_max
+                // FUCNTION Red led flickers fast
+                
+                //FUNCTION Measure EMG_max, EMG variable meten, t_passed
                 
                 StateChanged = false;
             }
@@ -86,43 +83,57 @@
         case MOVE_START:
             if(StateChanged)
             {
-                led1 = 1;
-                led2 = 1; 
-                led3 = 0;   // Green led is on
+                led_red = 1;
+                led_blue = 1; 
+                led_green = 0;   // Green led is on
             
-            // define start position
+                // FUNCTION move to start, t_passed
+                // Define current_position & start_position 
+                StateChanged = false; 
+            }
             
-            if //position realised and tmie passed
+            if (current_position == start_position && t_passed > 2) // FUNCTIO t_passed
             {
-                // Stop moving
                 CurrentState = READY_START;
                 StateChanged = true;
             } 
         break;      // end of state MOVE_START
         
         case READY_START:
-            while(true)
+            if (StateChanged)
             {
-                led1 = 1;
-                led2 = 1;
-                led3 = 0;
+                led_red = 1;
+                led_blue = 1;
+                led_green = 0;       // Green led is on 
+                
+                StateChanged = false;
             }
             
-            if (ptc1 == 0) 
+            if (button_1.read() == false)   // Button 1
             {
                 CurrentState = DEMO;
                 StateChanged = true;
+                wait(0.2f);
             }
-            else if (ptc2 == 0) // or EMG signal >= 20% max
+            else if (button_2.read() == false || EMG > 0.2*EMG_max) // Button 2 or 20% EMG_max
             {
                 CurrentState = MOVE;
                 StateChanged = true;
+                wait(0.2f);
             }                
         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 
+            if (StateChanged)
+            {
+                //FUNCTION Blue led blink fast
+                
+                //FUNCTION perform straight movements for demo
+                
+                StateChanged = false; 
+            }
+            
+            if (current_position == end_position && t_passed > 2) 
             {
                 CurrentState = MOVE_START;
                 StateChanged = true;
@@ -130,38 +141,69 @@
         break;      // end of state DEMO
         
         case MOVE:
-            if // no movement for 2 sec after input
+            if (StateChanged)
+            {
+                led_red = 1;
+                led_green = 1;
+                led_blue = 0;  //Blue led is on 
+                
+                // FUNCTION Play the game with EMG signal
+                
+                StateChanged = false; 
+            }
+            
+            if (button_Mbed.read() == false)
+            {
+                CurrentState = OFF;
+                StateChanged = true;
+                wait(0.2f);
+            }
+            
+            else if (button_1.read() == false || EMG < 0.2*EMG_max) 
             {
                 CurrentState = WAIT;
                 StateChanged = true;
+                wait(0.2f);
             }
-            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
+            break;      // end of state MOVE
         
         case WAIT:
-            if (ptc1 == 0)
+            if (StateChanged)
+            {
+                led_red = 0;   // Pink led iS on 
+                led_blue = 0; 
+                led_green = 1
+                
+            if (button_Mbed.read() == false) // For five second, too hard
             {
-                CurrentState = READY_START;
+                CurrentState = OFF;
                 StateChanged = true;
+                wait(0.2f);
             )
-            else if (ptc2 == 0) // or EMG>= 20%max
+            else if (button_1.read() == false) 
+            {
+                CurrentState = MOVE_START;
+                StateChanged = true;
+                wait(0.2f);
+            }
+            else if (button_2.read() == false || EMG > 0.2*EMG_max) 
             {
                 CurrentState = MOVE;
                 StateChanged = true;
+                wait(0.2f);
             }
-            else if (ptc1 == 0) //maar dan heel lang achter elkaar
+            
         break;      // end of state WAIT
         
         case OFF:
-            // turns all of 
+            led_red = 0;       // White led is on 
+            led_blue = 0; 
+            led_green = 0;
+            
         break;      // end of state OFF
         
         default:
-        TurnMotorsoff();
+        TurnMotorsoff(); //FUNCTION 
         printf("Unknown state reached");
     }               // End of the switch, all states are prescribed
 }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/QEI.lib	Mon Oct 14 12:14:18 2019 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/aberk/code/QEI/#5c2ad81551aa