Diff: main.cpp

Revision:

24:e87e4fcf6226

Parent:

23:ff73ee119244

Child:

25:45c131af2dba

--- a/main.cpp	Thu Oct 31 09:48:53 2019 +0000
+++ b/main.cpp	Thu Oct 31 18:53:30 2019 +0000
@@ -1,4 +1,3 @@
-// Ticker function.attach(nothing,1000) good solution??????
 // Operating mode might not go to next state when SW2 is pressed
 
 #include "mbed.h"
@@ -16,18 +15,27 @@
 DigitalIn but3(SW2);    // To go to the next state or to choose one of two game modes when in state START_GAME
 DigitalIn but4(SW3);    // To choose one of two game modes when in state START_GAME or to move the gripper
 
-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); // MotorVelocities of motors 0,1,2
-QEI E0(D8,D9,NC,8400), E1(D11,D10,NC,8400); // Encoders of motors 0,1,2
+AnalogIn S0(A0);
+AnalogIn S1(A1);
+AnalogIn S2(A2);
+AnalogIn S3(A3);
+
+DigitalOut motor1Direction(D7);
+FastPWM motor1Velocity(D6);
+DigitalOut motor2Direction(D4);
+FastPWM motor2Velocity(D5);
+
+// Encoders 1 and 2 respectively
+QEI Encoder1(D8,D9,NC,8400);
+QEI Encoder2(D11,D10,NC,8400);
 
 Ticker measurecontrol;              // Ticker function for the measurements
 
 // Make arrays for the different variables for the motors
-AnalogIn Shields[4] = {S0, S1, S2, S3};
-DigitalOut MotorDirections[2] = {MD0, MD1};
-FastPWM MotorVelocities[2] = {MV0, MV1};
-QEI Encoders[2] = {E0, E1};
+//AnalogIn Shields[4] = {S0, S1, S2, S3};
+//DigitalOut MotorDirections[2] = {MD0, MD1};
+//FastPWM MotorVelocities[2] = {MV0, MV1};
+//QEI Encoders[2] = {E0, E1};
 
 Serial pc(USBTX, USBRX);
 
@@ -40,7 +48,7 @@
 double xendeffector = 0;
 int ingedrukt = 0;
 int state_int;
-
+int previous_state_int;
 // Define the different states in which the robot can be
 enum States {MOTORS_OFF, EMG_CALIBRATION, START_GAME,
              DEMO_MODE, OPERATING_MODE, END_GAME
@@ -94,13 +102,10 @@
     pc.baud(115200);
     pc.printf("Starting...\r\n\r\n");
 
-    for (int i = 0; i < 2; i++) {
-        MotorVelocities[i].period(period_signal);   // Set the period of the PWMfunction
-    }
+        motor1Velocity.period(period_signal);   // Set the period of the PWMfunction
+        motor2Velocity.period(period_signal);   // Set the period of the PWMfunction
 
-    measurecontrol.attach(nothing, 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()
+    previous_state_int = (int)MyState;  // Save previous state to compare with current state and possibly execute New_State()
     // in the while loop
 
     New_State();                            // Execute the functions belonging to the current state
@@ -109,9 +114,6 @@
         if ( (previous_state_int - (int)MyState) != 0 ) {   // If current state is not previous state execute New_State()
             New_State();
         }
-
-        previous_state_int = (int)MyState-state_int;                  // Change previous state to current state
-        state_int = 0;
     }
 }
 
@@ -127,7 +129,7 @@
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
     // PID variables: we assume them to be the same for both motors
-    double Kp = 65;
+    double Kp = 63;
     double Ki = 3.64;
     double Kd = 5;
 
@@ -159,7 +161,7 @@
     static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241);
 
     // PID variables: we assume them to be the same for both motors
-    double Kp = 65;
+    double Kp = 63;
     double Ki = 3.64;
     double Kd = 5;
 
@@ -187,8 +189,8 @@
     // Obtain the counts of motors 1 and 2 from the encoder
     int countsmotor1;
     int countsmotor2;
-    countsmotor1 = Encoders[0].getPulses();
-    countsmotor2 = Encoders[1].getPulses();
+    countsmotor1 = Encoder1.getPulses();
+    countsmotor2 = Encoder2.getPulses();
 
     // Obtain the measured position for motor 1 and 2
     measuredposition1 = ((double)countsmotor1) / 8400.0f * 2.0f;
@@ -236,12 +238,12 @@
     absolutemotorvalue2 = absolutemotorvalue2 > 1.0f ? 1.0f : absolutemotorvalue2;
 
     // Send the absolutemotorvalue to the motors
-    MotorVelocities[0] = absolutemotorvalue1;
-    MotorVelocities[1] = absolutemotorvalue2;
+    motor1Velocity = absolutemotorvalue1;
+    motor2Velocity = absolutemotorvalue2;
 
     // Determine the motor direction. Boolean output: true gives counterclockwise direction, false gives clockwise direction
-    MotorDirections[0] = (motorvalue1 > 0.0f);
-    MotorDirections[1] = (motorvalue2 > 0.0f);
+    motor1Direction = (motorvalue1 > 0.0f);
+    motor2Direction = (motorvalue2 > 0.0f);
 }
 
 // function to call reference absolutemotorvalueocity, measured absolutemotorvalueocity and controls motor with feedback
@@ -315,10 +317,10 @@
     static BiQuad lFilter2(lb0,lb1,lb2,la0,la1,la2);
     static BiQuad lFilter3(lb0,lb1,lb2,la0,la1,la2);
 
-    f_y0 = hFilter0.step(Shields[0]);     // Apply filters on the different EMG signals
-    f_y1 = hFilter1.step(Shields[1]);
-    f_y2 = hFilter2.step(Shields[2]);
-    f_y3 = hFilter3.step(Shields[3]);
+    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);
@@ -358,14 +360,14 @@
 
     measure_data(y0, y1, y2, y3);   // Calculate RMS values
 
-    double duration = 20.0;                 // Duration of the emgcalibration function, in this case 10 seconds
+    double duration = 15.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 = (States)((int)MyState+1);    // If counter is larger than rounds, change MyState to the next state
-        measurecontrol.attach(nothing,10000);
+        MyState = START_GAME;    // If counter is larger than rounds, change MyState to the next state
+        measurecontrol.detach();
     } else {
         counter++;  // Else increase counter by 1
     }
@@ -432,9 +434,12 @@
 {
     double y0,y1,y2,y3;
     measure_data(y0,y1,y2,y3);
-
+    
+    y0 = 0;
+    y1 = 0;
+    
     double threshold = 0.3;     // When the analysed signal is above this threshold, the position of the end effector is changed
-    double dr = 0.1;            // The change in position with each step
+    double dr = 0.01;            // The change in position with each step
     
     if(y0 > threshold && xendeffector < 16) {
         xendeffector=xendeffector+dr;
@@ -454,39 +459,36 @@
     if(but4.read() == 0 && ingedrukt == 0) {
         for(int i=0; i<100; i++) {
             myservo = i/100.0;
-
-            wait(0.01);
         }
         ingedrukt = 1;
     } else if(but4.read() == 0 && ingedrukt == 1) {
         for(int i=100; i>0; i--) {
             myservo = i/100.0;
-
-            wait(0.01);
         }
+        
         ingedrukt = 0;
     }
+    
     if (but3.read() == 0) {
-        MyState = (States)((int)MyState+1);
-        wait(0.5f);
+        pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
+        MyState = END_GAME;
+        xendeffector=0.0;
+        yendeffector=10.6159;
     }
     measureandcontrol();
 }
 
 void endgame()
 {
-    pc.printf("The game has ended, will move the end effector to (0,0), put the motors off and will now return to the state START_GAME");
-    xendeffector=0.0;
-    yendeffector=10.6159;
-    wait(0.3f);
-    sendtomotor(0.0f,0.0f);
-    wait(0.1f);
-    measurecontrol.attach(nothing,10000);
+    wait(1);
+    measurecontrol.detach();    
     MyState = START_GAME;
 }
 
 void New_State()
 {
+    previous_state_int = (int)MyState;                  // Change previous state to current state
+
     switch (MyState) {
         case MOTORS_OFF :
             pc.printf("\r\nState: Motors turned off\r\n");
@@ -508,6 +510,7 @@
             pc.printf("\r\nState: Demo mode\r\n");
             measurecontrol.attach(measureandcontrol,timeinterval);
             demo_mode();
+            measurecontrol.detach();
             break;
 
         case OPERATING_MODE :
@@ -522,7 +525,7 @@
 
         default :
             pc.printf("\r\nDefault state: Motors are turned off\r\n");
-            measurecontrol.attach(nothing,10000);
+             
             sendtomotor(0.0,0.0);
             break;
     }
@@ -534,7 +537,7 @@
     yendeffector=10.6159;
     wait(0.3f);
     sendtomotor(0.0,0.0);                       // Stop the motors
-    measurecontrol.attach(nothing,10000);   // Stop the ticker function from running
+        // Stop the ticker function from running
 
     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 button SW2 is pressed");