Diff: main.cpp

Revision:

22:cce4dc5738af

Parent:

19:b3e224e0cb85

Child:

23:ff73ee119244

--- a/main.cpp	Tue Oct 29 14:47:59 2019 +0000
+++ b/main.cpp	Wed Oct 30 11:34:47 2019 +0000
@@ -14,34 +14,36 @@
 
 #include "Motor_Control.h"
 
-DigitalIn button1(D12);     // Button1 input to go to next state
+DigitalIn button1(D13);     // 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 ledg(LED_GREEN); // to test stuff
 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
-DigitalOut  MD0(D1), MD1(D2), MD2(D3); // MotorDirection of motors 0,1,2
-FastPWM MV0(D4), MV1(D5), MV2(D6); // MotorVelocities of motors 0,1,2
-QEI E0(D7,D8,NC,8400), E1(D9,D10,NC,8400), E3(D11,D13,NC,8400); // Encoders of motors 0,1,2
+AnalogIn S0(A0), S1(A1), S2(A2),S3(A3);    // Input EMG Shield 0,1,2,3
+DigitalOut  MD0(D7), MD1(D4); // MotorDirection of motors 0,1,2
+FastPWM MV0(D6), MV1(D5), MV2(D12); // MotorVelocities of motors 0,1,2
+QEI E0(D8,D9,NC,8400), E1(D10,D11,NC,8400); // Encoders of motors 0,1,2
 
 Ticker measurecontrol;              // Ticker function for motor in- and output
 
 // Make arrays for the different variables for the motors
 AnalogIn Shields[4] = {S0, S1, S2, S3};
-DigitalOut MotorDirections[3] = {MD0, MD1, MD2};
+DigitalOut MotorDirections[2] = {MD0, MD1};
 FastPWM MotorVelocities[3] = {MV0, MV1, MV2};
-QEI Encoders[3] = {E0, E1, E3};
+QEI Encoders[2] = {E0, E1};
 
-MODSERIAL pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
 
 float PI = 3.1415926f;//535897932384626433832795028841971693993;
 float timeinterval = 0.001f;        // Time interval of the Ticker function
 bool whileloop = true;              // Statement to keep the whileloop in the main function running
-                                    // While loop has to stop running when failuremode is activated
+// 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_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
@@ -59,7 +61,8 @@
 
 double P_controller(double error);
 
-void nothing(){
+void nothing()
+{
     // Do nothing
 }
 
@@ -76,33 +79,34 @@
 void failuremode();
 
 
-
-
 //__________________________________________________________________________________________________________________________________
 //__________________________________________________________________________________________________________________________________
 int main()
 {
+    pc.baud(115200);
     ledr = 1;
+    ledg = 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
     }
+
     measurecontrol.attach(measureandcontrol, timeinterval); // Ticker function to measure motor input and control the motors
-    
+
     int previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
-                                            // in the while loop
+    // in the while loop
+
     New_State();                            // Execute the functions belonging to the current state
-    
-    while(whileloop)
-    {
+
+    while(whileloop) {
         if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
             New_State();
         }
@@ -112,27 +116,32 @@
     pc.printf("Programm stops running\r\n");    // So show that the programm is quiting
     sendtomotor(0.0);                          // Set the motor velocity to 0
     measurecontrol.attach(nothing,10000);       // Attach empty function to Ticker (?? Appropriate solution ??)
+
     return 0;
 }
 //__________________________________________________________________________________________________________________________________
 //__________________________________________________________________________________________________________________________________
 
-void motorsoff() {
-    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed. 
+void motorsoff()
+{
+    // Function to turn the motors off. First state that the robot has. Robot will stay in this state untill button1 is pressed.
     // Robot will not return to this state anymore unless the user sets it back to this state with the keyboard input.
-    
+
     bool whileloop_boolean = true;  // Boolean for the while loop
     sendtomotor(0.0);              // Set motor velocity to 0
-    
+
     while (whileloop_boolean) {
         if (button1.read() == 0) {          // If button1 is pressed:
             MyState = EMG_CALIBRATION;      // set MyState to EMG_CALIBRATION and exit the while loop
             whileloop_boolean = false;      // by making whileloop_boolean equal to false
+        } else if (MyState != MOTORS_OFF) { // If the state is changed by keyboard input, exit the while loop
+            whileloop_boolean = false;
         }
     }
 }
 
-void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3) {
+void measure_data(double &f_y0, double &f_y1, double &f_y2, double &f_y3)
+{
     // High pass
     double hb0 = 0.9169;  // Coefficients from the following formula:
     double hb1 = -1.8338; //
@@ -140,7 +149,7 @@
     double ha0 = 1.0;     // H(z) = ----------------------
     double ha1 = -1.8268; //        a0 + a1 z^-1 + a2 z^-2
     double ha2 = 0.8407;  //
-    
+
     // Low pass
     double lb0 = 0.000083621;  // Coefficients from the following formula:
     double lb1 = 0.0006724; //
@@ -148,32 +157,32 @@
     double la0 = 1.0;     // H(z) = ----------------------
     double la1 = -1.9740; //        a0 + a1 z^-1 + a2 z^-2
     double la2 = 0.9743;  //
-    
+
     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);
     static BiQuad hFilter2(hb0,hb1,hb2,ha0,ha1,ha2);
     static BiQuad hFilter3(hb0,hb1,hb2,ha0,ha1,ha2);
-    
+
     static BiQuad lFilter0(lb0,lb1,lb2,la0,la1,la2);  // Create 4 equal filters used for the different EMG signals
     static BiQuad lFilter1(lb0,lb1,lb2,la0,la1,la2);
     static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
     static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
-    
+
     f_y0 = hFilter0.step(S0);     // Apply filters on the different EMG signals
     f_y1 = hFilter1.step(S1);
     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);
@@ -181,44 +190,43 @@
 
 
     if (MyState == EMG_CALIBRATION) {
-        
+
         det_max(f_y0, max_y0);       // Determine the maximum RMS value of the EMG signals during the EMG_CALIBRATION state
         det_max(f_y1, max_y1);
         det_max(f_y2, max_y2);
         det_max(f_y3, max_y3);
-        
-    }
-    else if ((int)MyState > 4) {
+
+    } else if ((int)MyState > 4) {
         f_y0 = f_y0/max_y0;     // Normalise the RMS value by dividing by the maximum RMS value
         f_y1 = f_y1/max_y1;     // This is done during the states with a value higher than 4, as this is when you start the operating mode
         f_y2 = f_y2/max_y2;
         f_y3 = f_y3/max_y3;
     }
-    
+
 }
 
-void det_max(double y, double &max_y) {
+void det_max(double y, double &max_y)
+{
     max_y = max_y < y ? y : max_y; // if max_rms is smaller than rms, set rms as new max_rms, otherwise keep max_rms
 }
 
-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");
+void emgcalibration()
+{
     double y0, y1, y2, y3;           // RMS values of the different EMG signals
 
     measure_data(y0, y1, y2, y3);   // Calculate RMS values
-    
+
     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
-    
+
     static int counter = 0;             // Counter which keeps track of the amount of times the function has executed
     if (counter >= rounds) {
         MyState = MOTOR_CALIBRATION;    // If counter is larger than rounds, change MyState to MOTOR_CALIBRATION
-    }
-    else {
+    } else {
         counter++;  // Else increase counter by 1
     }
-    
+
 }
 
 //P control implementation (behaves like a spring)
@@ -227,26 +235,25 @@
     double Kp = 17.5;
     //Proportional part:
     double u_k = Kp * error;
-    
+
     //sum all parts and return it
     return u_k;
 }
-
+/*
 void get_input(char c)
 {
     if (c == 'd') {
         MyState = DEMO_MODE;
-    }
-    else if (c == 'o') {
+    } else if (c == 'o') {
         MyState = OPERATING_MODE;
-    }
-    else {
-    pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
-    get_input(pc.getc());
+    } else {
+        pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
+        get_input(pc.getc());
     }
 }
-
-void startgame() {
+*/
+void startgame()
+{
     pc.printf("Please choose which game you would like to start:\r\n");
 
     pc.printf("- Press 'd' to start the demo mode\r\n     Demo mode is a mode in which the different movements of the robot are shown.\r\n");
@@ -256,112 +263,136 @@
     pc.printf("- Press 'o' to start the operating mode\r\n     The operating mode is a mode in which you can control the robot by flexing the muscles to which the electrodes are attached.\r\n");
     pc.printf("     You will be able to move the arm and use the gripper to try and grab and lift objects from the board to the container.\r\n\r\n");
 
-    get_input(pc.getc());
+    while (MyState == START_GAME) {
+        char c = pc.getc();
+        if (c == 'd') {
+            MyState = DEMO_MODE;
+        } else if (c == 'o') {
+            MyState = OPERATING_MODE;
+        }
+        else {
+            pc.printf("'d' or 'o' were not pressed\r\nPress 'd' to start the demo mode and press 'o' to start the operating mode\r\n");
+        }
+    }
 }
 
 /*
 void demo_mode() {
-    
+
 }
 */
 
-void operating_mode() {
-        double y0,y1,y2,y3;
-        measure_data(y0,y1,y2,y3);
-        double F_Y[4] = {y0, y1, y2, y3};
-        
-        double threshold = 0.3;
-        for (int i = 0; i < 4; i++) {
-            if (F_Y[i] > threshold) {
-                //The direction and velocity of the motors are determind here
-            }
-        } 
-    
+void operating_mode()
+{
+    double y0,y1,y2,y3;
+    measure_data(y0,y1,y2,y3);
+    double F_Y[4] = {y0, y1, y2, y3};
+
+    double threshold = 0.3;
+    for (int i = 0; i < 4; i++) {
+        if (F_Y[i] > threshold) {
+            //The direction and velocity of the motors are determind here
+        }
+    }
+
 }
 
-void New_State() {
-    switch (MyState)
-    {
+void New_State()
+{
+    switch (MyState) {
         case MOTORS_OFF :
-            pc.printf("State: Motors turned off\r\n");
+            pc.printf("\r\nState: Motors turned off\r\n");
             motorsoff();
             break;
-        
+
         case EMG_CALIBRATION :
-            pc.printf("State: EMG Calibration\r\n");
+            pc.printf("\r\nState: EMG Calibration\r\n");
+            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");
             measurecontrol.attach(emgcalibration,timeinterval);
             break;
-        
+
         case MOTOR_CALIBRATION :
-            pc.printf("State: Motor Calibration\r\n");
+            pc.printf("\r\nState: Motor Calibration\r\n");
             break;
-        
+
         case START_GAME :
-            pc.printf("State: Start game\r\n");
+            pc.printf("\r\nState: Start game\r\n");
             startgame();
             break;
-        
+
         case DEMO_MODE :
-            pc.printf("State: Demo mode\r\n");
+            pc.printf("\r\nState: Demo mode\r\n");
             //demo_mode();
             break;
-        
+
         case OPERATING_MODE :
-            pc.printf("State: Operating mode\r\n");
+            pc.printf("\r\nState: Operating mode\r\n");
             measurecontrol.attach(operating_mode,timeinterval);
             break;
-        
+
         case MOVE_GRIPPER :
-            pc.printf("State: Move the gripper\r\n");
+            pc.printf("\r\nState: Move the gripper\r\n");
             break;
-        
+
         case END_GAME :
-            pc.printf("State: End of the game\r\n");
+            pc.printf("\r\nState: End of the game\r\n");
             break;
-        
+
         case FAILURE_MODE :
-            pc.printf("FAILURE MODE!!\r\n");        // Let the user know it is in failure mode
+            pc.printf("\r\nFAILURE 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\r\n");
+            pc.printf("\r\nDefault state: Motors are turned off\r\n");
+            measurecontrol.attach(nothing,10000);
             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");
+void Set_State()
+{
+    if (MyState == FAILURE_MODE) {
+        pc.printf("\r\nNot possible to set state because robot is in FAILURE_MODE. Please restart robot.\r\n");
+    } else {
+        sendtomotor(0.0);                       // Stop the motors
+        measurecontrol.attach(nothing,10000);   // Stop the ticker function from running
 
-    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");
+        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");
+
+        wait(0.5);
+
+        char a = '0';
+        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() {
+void failuremode()
+{
     MyState = FAILURE_MODE; // failurmode() is activated so set MyState to FAILURE_MODE
     New_State();            // Execute actions coupled to FAILURE_MODE
 }
\ No newline at end of file