Diff: main.cpp

Revision:

19:b3e224e0cb85

Parent:

18:4a6be1893d7f

Child:

22:cce4dc5738af

--- a/main.cpp	Tue Oct 29 11:17:48 2019 +0000
+++ b/main.cpp	Tue Oct 29 14:47:59 2019 +0000
@@ -9,13 +9,14 @@
 #include "FastPWM.h"
 #include "QEI.h"
 
-#include <math.h>
+#include <cmath>
 #include <deque>
 
 #include "Motor_Control.h"
 
 DigitalIn button1(D12);     // Button1 input to go to next state
 InterruptIn button2(SW2);   // Button2 input to activate failuremode()
+InterruptIn button3(SW3);   // Button3 input to go to a specific state
 DigitalOut ledr(LED_RED);   // Red LED output to show
 
 AnalogIn S0(A0), S1(A1), S2(A2) ,S3(A3);   // Input EMG Shield 0,1,2,3
@@ -39,9 +40,9 @@
                                     // While loop has to stop running when failuremode is activated
 
 // Define the different states in which the robot can be
-enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION,
-    START_GAME, DEMO_MODE, OPERATING_MODE, MOVE_END_EFFECTOR,
-    MOVE_GRIPPER, END_GAME, FAILURE_MODE};
+enum States {MOTORS_OFF, EMG_CALIBRATION, MOTOR_CALIBRATION, START_GAME, 
+    DEMO_MODE, OPERATING_MODE, MOVE_GRIPPER, END_GAME, FAILURE_MODE};
+
 
 // Default state is the state in which the motors are turned off
 States MyState = MOTORS_OFF;
@@ -70,6 +71,8 @@
 
 void New_State();
 
+void Set_State();
+
 void failuremode();
 
 
@@ -79,12 +82,15 @@
 //__________________________________________________________________________________________________________________________________
 int main()
 {
+    ledr = 1;
     pc.printf("Starting...\r\n\r\n");
     double frequency = 17000.0;            // Set motorfrequency
     double period_signal = 1.0/frequency;  // Convert to period of the signal
     pc.baud(115200);
-    
+    ledr = 0;
+    wait(5);
     button2.fall(failuremode);              // Function is always activated when the button is pressed
+    button3.fall(Set_State);                // Function is always activated when the button is pressed
     
     for (int i = 0; i < 3; i++) {
         MotorVelocities[i].period(period_signal);   // Set the period of the PWMfunction
@@ -143,10 +149,10 @@
     double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
     double la2 = 0.9743;  //
     
-    static double max_y0 = 0;
-    static double max_y1 = 0;
-    static double max_y2 = 0;
-    static double max_y3 = 0;
+    static double max_y0 = 1;
+    static double max_y1 = 1;
+    static double max_y2 = 1;
+    static double max_y3 = 1;
     
     static BiQuad hFilter0(hb0,hb1,hb2,ha0,ha1,ha2);  // Create 4 equal filters used for the different EMG signals
     static BiQuad hFilter1(hb0,hb1,hb2,ha0,ha1,ha2);
@@ -163,6 +169,11 @@
     f_y2 = hFilter2.step(S2);
     f_y3 = hFilter3.step(S3);
     
+    f_y0 = abs(f_y0);
+    f_y1 = abs(f_y1);
+    f_y2 = abs(f_y2);
+    f_y3 = abs(f_y3);
+    
     f_y0 = lFilter0.step(f_y0);
     f_y1 = lFilter1.step(f_y1);
     f_y2 = lFilter2.step(f_y2);
@@ -191,11 +202,12 @@
 }
 
 void emgcalibration() {
+    pc.printf("Emg calibration mode will run for 20 seconds. Try to flex the muscles to which the electrodes are attached as hard as possible during this time.\r\n");
     double y0, y1, y2, y3;           // RMS values of the different EMG signals
 
     measure_data(y0, y1, y2, y3);   // Calculate RMS values
     
-    double duration = 10.0;                 // Duration of the emgcalibration function, in this case 10 seconds
+    double duration = 20.0;                 // Duration of the emgcalibration function, in this case 10 seconds
     int rounds = (duration / timeinterval); // Determine the amount of times this function has to run to run for the duration time
     // rounds is an integer so the value of duration / timeinterval is floored
     
@@ -258,10 +270,10 @@
         measure_data(y0,y1,y2,y3);
         double F_Y[4] = {y0, y1, y2, y3};
         
-        double threshold = 0.5;
+        double threshold = 0.3;
         for (int i = 0; i < 4; i++) {
             if (F_Y[i] > threshold) {
-                //hey
+                //The direction and velocity of the motors are determind here
             }
         } 
     
@@ -271,59 +283,84 @@
     switch (MyState)
     {
         case MOTORS_OFF :
-            pc.printf("State: Motors turned off");
+            pc.printf("State: Motors turned off\r\n");
             motorsoff();
             break;
         
         case EMG_CALIBRATION :
-            pc.printf("State: EMG Calibration");
+            pc.printf("State: EMG Calibration\r\n");
             measurecontrol.attach(emgcalibration,timeinterval);
             break;
         
         case MOTOR_CALIBRATION :
-            pc.printf("State: Motor Calibration");
+            pc.printf("State: Motor Calibration\r\n");
             break;
         
         case START_GAME :
-            pc.printf("State: Start game");
+            pc.printf("State: Start game\r\n");
             startgame();
             break;
         
         case DEMO_MODE :
-            pc.printf("State: Demo mode");
+            pc.printf("State: Demo mode\r\n");
             //demo_mode();
             break;
         
         case OPERATING_MODE :
-            pc.printf("State: Operating mode");
+            pc.printf("State: Operating mode\r\n");
             measurecontrol.attach(operating_mode,timeinterval);
             break;
         
-        case MOVE_END_EFFECTOR :
-            pc.printf("State: Move end effector");
-            break;
-        
         case MOVE_GRIPPER :
-            pc.printf("State: Move the gripper");
+            pc.printf("State: Move the gripper\r\n");
             break;
         
         case END_GAME :
-            pc.printf("State: End of the game");
+            pc.printf("State: End of the game\r\n");
             break;
         
         case FAILURE_MODE :
-            pc.printf("FAILURE MODE!!");            // Let the user know it is in failure mode
+            pc.printf("FAILURE MODE!!\r\n");        // Let the user know it is in failure mode
             ledr = 0;                               // Turn red led on to show that failure mode is active
             whileloop = false;
             break;
         
         default :
-            pc.printf("Default state: Motors are turned off");
+            pc.printf("Default state: Motors are turned off\r\n");
             sendtomotor(0.0);
             break;
     }
 }
 
+void Set_State() {
+    pc.printf("\r\nPress number:     | To go to state:");
+    pc.printf("\r\n              (0) | MOTORS_OFF: Set motorspeed just in case to 0 and wait till button1 is pressed");
+    pc.printf("\r\n              (1) | EMG_CALIBRATION: Calibrate the maximum of the emg signals and normalise the emg signals with these maxima");
+    pc.printf("\r\n              (2) | MOTOR_CALIBRATION: Calibrate the motors to determine the (0,0) position.");
+    pc.printf("\r\n              (3) | START_GAME: Determine by keyboard input if you want to go to the demo or operating mode");
+    pc.printf("\r\n              (4) | DEMO_MODE: The demo mode will show the different motions that the robot can make");
+    pc.printf("\r\n              (5) | OPERATING_MODE: Move the arms and gripper of the arm by flexing your muscles");
+    pc.printf("\r\n              (6) | MOVE_GRIPPER: Control the movement of the gripper");
+    pc.printf("\r\n              (7) | END_GAME: End effector returns to (0,0) and the motors are turned off, returns to START_GAME mode afterwards");
+    pc.printf("\r\n              (8) | FAILURE_MODE: Everything is stopped and the whole programm stops running.\r\n");
+
+    char a = '1';
+    char b = '8';
+    bool boolean = true;
+    
+    while (boolean) {
+        char c = pc.getc();
+    
+        if (c >= a && c <= b) {
+            MyState = (States)(c-'0');
+            boolean = false;
+        }
+        else { pc.printf("\r\nPlease enter a number between 0 and 8\r\n");
+        }
+    }
+    New_State();
+}
+
 void failuremode() {
     MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
     New_State();            // Execute actions coupled to FAILURE_MODE