Diff: main.cpp

Revision:

6:f495a77c2c95

Parent:

5:3581013d4505

Child:

7:ec5add330cb3

--- a/main.cpp	Mon Oct 22 09:33:18 2018 +0000
+++ b/main.cpp	Mon Oct 22 13:51:40 2018 +0000
@@ -1,6 +1,7 @@
 #include "mbed.h" // Use revision 119!!
 #include "HIDScope.h" // For displaying data, select MBED - HID device, restart for every new code 
 #include "QEI.h" // For reading the encoder of the motors 
+#include <ctime> // for the timer in the states 
 
 #define SERIAL_BAUD 115200
 
@@ -40,15 +41,24 @@
 // -----------------------------------------------------------------------------
 // Define stuff like tickers etc
 
-// Ticker NAME; // Name a ticker, use each ticker only for 1 function! 
+//Ticker NAME; // Name a ticker, use each ticker only for 1 function! 
 HIDScope scope(2); // Number of channels in HIDScope
 QEI Encoder(D13,D12,NC,64,QEI::X4_ENCODING); // Define the type of encoding: X4 encoding(default is X2)
 Serial pc(USBTX,USBRX);
+Timer t; // For timing the time in each state (https://os.mbed.com/handbook/Timer)
 
 // Variables 
 // ----------------------------------------------------------------------------- 
 // Define here all variables needed throughout the whole code 
 int counts;
+float potmeter_value;
+double time;
+float time_in_state;
+
+// states
+enum states (WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO) // states the robot can be in
+states CurrentState = WAIT; // the current state to start with is the WAIT state
+bool StateChanged = true; // the state must be changed to go into the next state
 
 // Functions
 // -----------------------------------------------------------------------------
@@ -74,7 +84,7 @@
     pwmpin_2 = fabs (out_2);
         
     return out_2;
-}
+    }
 
 // Motor calibration
 // To calibrate the motor angle to some mechanical boundaries
@@ -82,16 +92,17 @@
 
 // EMG calibration
 // To calibrate the EMG signal to some boundary values
+// Simon mee bezig 
 
 // Send EMG to HIDScope
-void sample() // Attach this to a ticker! 
+void hidscope() // Attach this to a ticker! 
 {
-    scope.set(0, counts); // send counts to channel 1 (=0)
-    // scope.set(1, velocity_encoder() ); // sent EMG 2 to channel 2 (=1)
+    scope.set(0, counts); // send EMG 1 to channel 1 (=0)
+    scope.set(1, potmeter_value); // sent EMG 2 to channel 2 (=1)
     
     // Ensure that enough channels are available (HIDScope scope(2))
     scope.send(); // Send all channels to the PC at once
-}
+    }
 
 // Start
 // To move the robot to the starting position: middle
@@ -102,10 +113,11 @@
 
 // Operating mode
 // To control the robot with EMG signals
+// Gertjan bezig met motor aansturen
 
 // Processing EMG 
 // To process the raw EMG to a usable value, with filters etc
-// Simon en Kees mee bezig 
+// Kees mee bezig 
 
 // Demo mode
 // To control the robot with a written code and write 'HELLO'
@@ -121,26 +133,170 @@
     pc.baud(115200); // For TeraTerm, the baudrate, set also in TeraTerm itself! 
     pc.printf("Start code\r\n"); // To check if the program starts 
     pwmpin_1.period_us(60); // Needed for PWM, not exactly known why
+    NAME.attach(&hidscope(),0.002);
     
     while(true){
+        // timer
+        clock_t start; // start the timer
+        start = clock(); 
+        time = (clock() - start) / (double) CLOCKS_PER_SEC;
         
         counts = encoder();
-        float out_2 = potmeter();
-        sample(); 
+        potmeter_value = potmeter();
+         
         
-        pc.printf("potmeter value = %f ", out_2);
-        pc.printf("counts = %i\r\n", counts); 
+        //pc.printf("potmeter value = %f ", potmeter_value);
+        //pc.printf("counts = %i\r\n", counts); 
         
-        /* if (button_motorcal == true){
-            // execute motor calibration
+        // With the help of a switch loop and states we can switch between states and the robot knows what to do
+        switch{CurrentState}
+        {
+            case WAIT:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute WAIT mode
+                pc.printf("State is WAIT\r\n");
+                StateChanged = false; // the state is still WAIT
+                }
+                
+            if() // condition for WAIT --> MOTOR_CAl; button press
+            {
+                CurrentState = MOTOR_CAL;
+                StateChanged = true;
+                }
+            
+            break;
+            
+            case MOTOR_CAL:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.start();
+                // Execute MOTOR_CAL mode
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("State is MOTOR_CAL\r\n");
+                StateChanged = false; // the state is still MOTOR_CAL
+                }
+            
+            if(time_in_state > 3.0 && motor_velocity < 0.01) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
+            {
+                CurrentState = EMG_CAL;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case EMG_CAL:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.reset();
+                t.start();
+                // Execute EMG_CAL mode
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("State is EMG_CAL\r\n");
+                StateChanged = false; // state is still EMG_CAL
+                }
+            
+            if(time_in_state < 5 && EMG < 0.01) // condition for EMG_CAL --> START; 5s and EMG is low
+            {
+                CurrentState = START;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case START:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                t.reset();
+                t.start();
+                // Execute START mode
+                t.stop();
+                time_in_state = t.read();
+                pc.printf("State is START\r\n");
+                StateChanged = false; // state is still START
+                }
+            
+            if(time_in_state < 5 && error < 0.01) // condition for START --> OPERATING; 5s and error is small
+            {
+                CurrentState = OPERATING;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case OPERATING:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute OPERATING mode
+                pc.printf("State is OPERATING\r\n");
+                StateChanged = false; // state is still OPERATING
+                }
+            
+            if() // condition for OPERATING --> FAILURE; button press
+            {
+                CurrentState = FAILURE;
+                StateChanged = true;
+                }
+                
+            if() // condition for OPERATING --> DEMO; button press
+            {
+                CurrentState = DEMO;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case FAILURE:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute FAILURE mode
+                pc.printf("State is FAILURE\r\n");
+                StateChanged = false; // state is still FAILURE
+                }
+            
+            if() // condition for FAILURE --> WAIT; button press 
+            {
+                CurrentState = WAIT;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            case DEMO:
+            
+            if(StateChanged) // so if StateChanged is true
+            {
+                // Execute DEMO mode
+                pc.printf("State is DEMO\r\n");
+                StateChanged = false; // state is still DEMO
+                }
+            
+            if() // condition for DEMO --> WAIT; button press 
+            {
+                CurrentState = WAIT;
+                StateChanged = true;
+                }
+                
+            if () // condition for DEMO --> FAILURE; button press 
+            {
+                CurrentState = FAILURE;
+                StateChanged = true;
+                }
+                
+            break;
+            
+            // no default  
             }
-        elseif {
-            // remain in waiting mode 
-            break;
-            }
-            */
-        
-        wait(0.01); // loop the while loop every 0.01 seconds
+            
+        // while loop does not have to loop every time
         }
         
 }
\ No newline at end of file