Diff: main.cpp

Revision:

9:d7a6a3619576

Parent:

8:5ad8a7892693

Child:

10:56136a0da8c1

--- a/main.cpp	Mon Oct 22 14:26:34 2018 +0000
+++ b/main.cpp	Fri Oct 26 09:40:09 2018 +0000
@@ -1,7 +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 
+#include <ctime> // for the timer during the process (if needed)
 
 #define SERIAL_BAUD 115200
 
@@ -25,15 +25,16 @@
 
 // Extra stuff
 // Like LED lights, buttons etc 
-/*
-DigitalIn button_motorcal(SW1); // button for motor calibration, on mbed
+
+DigitalIn button_motorcal(SW2); // button for motor calibration, on mbed
 DigitalIn button_emergency(D7); // button for emergency mode, on bioshield
-DigitalIn button_wait(SW2); // button for wait mode, on mbed 
+DigitalIn button_wait(SW3); // button for wait mode, on mbed 
 DigitalIn button_demo(D6); // button for demo mode, on bioshield  
-*/
+
 DigitalIn led_red(LED_RED); // red led 
 DigitalIn led_green(LED_GREEN); // green led
 DigitalIn led_blue(LED_BLUE); // blue led
+
 AnalogIn pot_1(A1); // potmeter for simulating EMG input
 
 
@@ -41,7 +42,7 @@
 // -----------------------------------------------------------------------------
 // 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);
@@ -52,11 +53,14 @@
 // Define here all variables needed throughout the whole code 
 int counts;
 float potmeter_value;
-double time;
+double time_overall;
 float time_in_state;
+double motor_velocity = 0;
+double EMG = 0;
+double errors = 0;
 
 // states
-enum states (WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO); // states the robot can be in
+enum states {WAIT, MOTOR_CAL, EMG_CAL, START, OPERATING, FAILURE, DEMO}; // states the robot can be in
 states CurrentState = WAIT; // the CurrentState to start with is the WAIT state
 bool StateChanged = true; // the state must be changed to go into the next state
 
@@ -85,6 +89,10 @@
         
     return out_2;
     }
+    
+// EMG filter
+// To process the EMG signal before information can be caught from it 
+// Kees mee bezig 
 
 // Motor calibration
 // To calibrate the motor angle to some mechanical boundaries
@@ -94,7 +102,7 @@
 // To calibrate the EMG signal to some boundary values
 // Simon mee bezig 
 
-// Send EMG to HIDScope
+// Send information to HIDScope
 void hidscope() // Attach this to a ticker! 
 {
     scope.set(0, counts); // send EMG 1 to channel 1 (=0)
@@ -103,6 +111,10 @@
     // Ensure that enough channels are available (HIDScope scope(2))
     scope.send(); // Send all channels to the PC at once
     }
+    
+// PID controller
+// To control the input signal before it goes into the motor control
+// Simon mee bezig
 
 // Start
 // To move the robot to the starting position: middle
@@ -115,10 +127,6 @@
 // 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
-// Kees mee bezig 
-
 // Demo mode
 // To control the robot with a written code and write 'HELLO'
 // Voor het laatst bewaren
@@ -132,14 +140,14 @@
 {
     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);
+    pwmpin_1.period_us(60); // Setting period for PWM
+    NAME.attach(&hidscope,0.002f); // Send information to HIDScope
     
     while(true){
         // timer
         clock_t start; // start the timer
         start = clock(); 
-        time = (clock() - start) / (double) CLOCKS_PER_SEC;
+        time_overall = (clock() - start) / (double) CLOCKS_PER_SEC;
         
         counts = encoder();
         potmeter_value = potmeter();
@@ -149,7 +157,7 @@
         //pc.printf("counts = %i\r\n", counts); 
         
         // With the help of a switch loop and states we can switch between states and the robot knows what to do
-        switch{CurrentState}
+        switch(CurrentState)
         {
             case WAIT:
             
@@ -163,12 +171,14 @@
             if(button_motorcal == true) // condition for WAIT --> MOTOR_CAl; button_motorcal press
             {
                 CurrentState = MOTOR_CAL;
+                pc.printf("State is MOTOR_CAL\r\n");
                 StateChanged = true;
                 }
                 
             if (button_emergency == true) // condition for WAIT --> FAILURE; button_emergency press 
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }
             
@@ -178,23 +188,26 @@
             
             if(StateChanged) // so if StateChanged is true
             {
+                t.reset();
                 t.start();
                 // Execute MOTOR_CAL mode
                 t.stop();
                 time_in_state = t.read();
-                pc.printf("State is MOTOR_CAL\r\n");
+                pc.printf("Time here = %f\r\n", time_in_state);
                 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
+            if((time_in_state > 3.0) && (motor_velocity < 0.01)) // condition for MOTOR_CAL --> EMG_CAL; 3s en motors stopped moving
             {
                 CurrentState = EMG_CAL;
+                pc.printf("State is EMG_CAL\r\n");
                 StateChanged = true;
                 }
                 
             if (button_emergency == true) // condition for MOTOR_CAL --> FAILURE; button_emergency press 
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }
                 
@@ -209,19 +222,21 @@
                 // Execute EMG_CAL mode
                 t.stop();
                 time_in_state = t.read();
-                pc.printf("State is EMG_CAL\r\n");
+                pc.printf("Time here = %f\r\n", time_in_state);
                 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
+            if((time_in_state > 5) && (EMG < 0.01)) // condition for EMG_CAL --> START; 5s and EMG is low
             {
                 CurrentState = START;
+                pc.printf("State is START\r\n");
                 StateChanged = true;
                 }
                 
             if (button_emergency == true) // condition for EMG_CAL --> FAILURE; button_emergency press 
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }
                 
@@ -236,19 +251,21 @@
                 // Execute START mode
                 t.stop();
                 time_in_state = t.read();
-                pc.printf("State is START\r\n");
+                pc.printf("Time here = %f\r\n", time_in_state);
                 StateChanged = false; // state is still START
                 }
             
-            if(time_in_state < 5 && error < 0.01) // condition for START --> OPERATING; 5s and error is small
+            if((time_in_state > 5) && (errors < 0.01)) // condition for START --> OPERATING; 5s and error is small
             {
                 CurrentState = OPERATING;
+                pc.printf("State is OPERATING\r\n");
                 StateChanged = true;
                 }
                 
             if (button_emergency == true) // condition for START --> FAILURE; button_emergency press 
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }
                 
@@ -266,12 +283,21 @@
             if(button_emergency == true) // condition for OPERATING --> FAILURE; button_emergency press
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }
                 
             if(button_demo == true) // condition for OPERATING --> DEMO; button_demo press
             {
                 CurrentState = DEMO;
+                pc.printf("State is DEMO\r\n");
+                StateChanged = true;
+                }
+                
+            if(button_wait == true) // condition OPERATING --> WAIT; button_wait press  
+            {
+                CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
                 StateChanged = true;
                 }
                 
@@ -289,6 +315,7 @@
             if(button_wait == true) // condition for FAILURE --> WAIT; button_wait press (IF THAT IS EVEN POSSIBLE IN THIS STATE?) 
             {
                 CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
                 StateChanged = true;
                 }
                 
@@ -306,12 +333,14 @@
             if(button_wait == true) // condition for DEMO --> WAIT; button_wait press 
             {
                 CurrentState = WAIT;
+                pc.printf("State is WAIT\r\n");
                 StateChanged = true;
                 }
                 
             if (button_emergency == true) // condition for DEMO --> FAILURE; button_emergency press 
             {
                 CurrentState = FAILURE;
+                pc.printf("State is FAILURE\r\n");
                 StateChanged = true;
                 }