code die in het verslag komt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

Files at this revision

API Documentation at this revision

Comitter:
RiP
Date:
Mon Nov 07 13:17:21 2016 +0000
Parent:
8:9dcd1603d713
Commit message:
totale code

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/main.cpp	Sun Nov 06 13:28:32 2016 +0000
+++ b/main.cpp	Mon Nov 07 13:17:21 2016 +0000
@@ -6,87 +6,89 @@
 #include "FastPWM.h"
 
 // In use: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
-// 8(pushbutton),9(servoPWM),10(encoder motor 2),11(encoder motor 2),12(encoder motor 1),13(encoder motor 1)) A(0,1,2)(EMG)
+// 8(pushbutton),9(servoPWM),10(encoder motor 2),11(encoder motor 2),
+// 12(encoder motor 1),13(encoder motor 1)) A(0,1,2)(EMG)
 
 MODSERIAL   pc(USBTX, USBRX);
 
 // Define the EMG inputs
-AnalogIn    emg_in1( A0 );                                               // EMG signal of the thumb
-AnalogIn    emg_in2( A1 );                                               // EMG signal of the right bicep
-AnalogIn    emg_in3( A2 );                                               // EMG signal of the left bicep
+AnalogIn    emg_in1( A0 );  // EMG signal of the thumb
+AnalogIn    emg_in2( A1 );  // EMG signal of the right bicep
+AnalogIn    emg_in3( A2 );  // EMG signal of the left bicep
 
 // Define motor outputs
-DigitalOut  motor1dir(D7);                                               // Direction of motor 1, attach at m1, set to 0: cw
-DigitalOut  motor2dir(D4);                                               // Direction of motor 2, attach at m2, set to 0: ccw
-FastPWM     motor1(D6);                                                  // Duty cycle of pwm signal for motor 1
-FastPWM     motor2(D5);                                                  // Duty cycle of pwm signal for motor 2
-FastPWM     servo(D9);                                                   // Pulsewidth of pwm signal for servo
+DigitalOut  motor1dir(D7);  // Direction of motor 1, attach at m1, set to 0: cw
+DigitalOut  motor2dir(D4);  // Direction of motor 2, attach at m2, set to 0: ccw
+FastPWM     motor1(D6);     // Duty cycle of pwm signal for motor 1
+FastPWM     motor2(D5);     // Duty cycle of pwm signal for motor 2
+FastPWM     servo(D9);      // Pulsewidth of pwm signal for servo
 
 // Define button for flipping the spatula
 DigitalIn   servo_button(PTC12);
 
 // Define encoder inputs
-QEI         encoder1(D13,D12,NC,64,QEI::X4_ENCODING);                   //Defining highest encoder accuracy for motor1
-QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);                   //Defining highest encoder accuracy for motor2
+QEI         encoder1(D13,D12,NC,64,QEI::X4_ENCODING);   //Defining highest encoder accuracy for motor1
+QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);   //Defining highest encoder accuracy for motor2
 
 // Define the Tickers
-Ticker      print_timer;                                                // Ticker for printing position or highest EMG values
-Ticker      controller_timer;                                           // Ticker for sampling and motor control
-Ticker      servo_timer;                                                // Ticker for servo control
+Ticker      print_timer;        // Ticker for printing position or highest EMG values
+Ticker      controller_timer;   // Ticker for sampling and motor control
+Ticker      servo_timer;        // Ticker for servo control
 
 // Define the Time constants
-const double  Ts = 0.002;                                               // Time constant for sampling and motor control
-const double  servo_Ts = 0.02;                                          // Time constant for servo control
+const double  Ts = 0.002;       // Time constant for sampling and motor control
+const double  servo_Ts = 0.02;  // Time constant for servo control
 
 // Define the go flags
-volatile bool controller_go = false;                                    // Go flag for sample() and motor_controller()
-volatile bool servo_go = false;                                         // Go flag servo_controller()
+volatile bool controller_go = false;    // Go flag for sample() and motor_controller()
+volatile bool servo_go = false;         // Go flag servo_controller()
 
 // Define the EMG variables
-double emg1, emg2, emg3;                                                // The three filtered EMG signals
-double highest_emg1, highest_emg2, highest_emg3;                        // The highest EMG signals of emg_in
-double threshold1, threshold2, threshold3;                              // The threshold for the EMG signals to change the reference
+double emg1, emg2, emg3;                         // The three filtered EMG signals
+double highest_emg1, highest_emg2, highest_emg3; // The highest EMG signals of emg_in
+double threshold1, threshold2, threshold3;       // The threshold for the EMG signals to change the reference
 
 //Define the keyboard input
-char key;                                                               // Stores last pressed key
+char key;                           // Stores last pressed key
 
 // Define the reference variables
-double ref_x = 0.0, ref_y = 0.0;                                        // Reference position
-double old_ref_x, old_ref_y;                                            // Old reference position
-double speed = 0.00006;                                                 // Variable with which a speed is reached of 3 cm/s in x and y direction
-double theta = 0.0;                                                     // Angle reference of the arm
-double radius = 0.0;                                                    // Radius reference of the arm
-bool   z_pushed = false;                                                // To see if z is pressed
+double ref_x = 0.0, ref_y = 0.0;    // Reference position
+double old_ref_x, old_ref_y;        // Old reference position
+double speed = 0.00006;             // Variable with which a speed is reached of 3 cm/s in x and y direction
+double theta = 0.0;                 // Angle reference of the arm
+double radius = 0.0;                // Radius reference of the arm
+bool   z_pushed = false;            // To see if z is pressed
 
 // Define reference limits
-const double min_radius = 0.43;                                         // The minimum radius of the arm
-const double max_radius = 0.62;                                         // The maximum radius of the arm
-const double min_y = -0.26;                                             // The minimum y position of the arm
+const double min_radius = 0.43;     // The minimum radius of the arm
+const double max_radius = 0.62;     // The maximum radius of the arm
+const double min_y = -0.26;         // The minimum y position of the arm
 
 // Define variables of motor 1
-double m1_pwm = 0;                                                      // Variable for PWM  motor 1
+double m1_pwm = 0;                  // Variable for PWM  motor 1
 const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100;    // PID values for motor 1
-double m1_v1 = 0, m1_v2 = 0;                                            // Memory variables
+double m1_v1 = 0, m1_v2 = 0;        // Memory variables
 
 // Define variables of motor 2
-double m2_pwm = 0;                                                      // Variable for PWM motor 2
+double m2_pwm = 0;                  // Variable for PWM motor 2
 const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100;   // PID values for motor 2
-double m2_v1 = 0, m2_v2 = 0;                                            // Memory variables
+double m2_v1 = 0, m2_v2 = 0;        // Memory variables
 
 // Define machine constants
 const double pi = 3.14159265359;
-const double res = 64.0 / (1.0 / 131.25 * 2.0 * pi);                    // Resolution on gearbox shaft per pulse
-const double V_max = 9.0;                                               // Maximum voltage supplied by trafo
-const double pulley_radius = 0.0398/2.0;                                // Pulley radius
+const double res = 64.0 / (1.0 / 131.25 * 2.0 * pi);    // Resolution on gearbox shaft per pulse
+const double V_max = 9.0;                               // Maximum voltage supplied by trafo
+const double pulley_radius = 0.0398/2.0;                // Pulley radius
 
 // Define variables needed for controlling the servo
-double       servo_pwm = 0.0023;                                        // Pulsewidth PWM for servo (min 0.0005, max 0.0025)
-const double min_theta = -37.0 / 180.0 * pi;                            // Minimum angle of the arm
-const double max_theta = -14.0 / 180.0 * pi;                            // Maximum angle to which the spatula is stabilised
-const double diff_theta = max_theta - min_theta;                        // Difference between max and min angle
-const double min_servo_pwm = 0.0021;                                    // Corresponds to angle of theta -38 degrees
-const double max_servo_pwm = 0.0024;                                    // Corresponds to angle of theta -24 degrees
-const double res_servo = max_servo_pwm - min_servo_pwm;                 // Resolution of servo pwm signal between min and max angle
+double       servo_pwm = 0.0023;                        // Pulsewidth PWM for servo (min 0.0005, max 0.0025)
+const double min_theta = -37.0 / 180.0 * pi;            // Minimum angle of the arm
+const double max_theta = -14.0 / 180.0 * pi;            // Maximum angle to which the spatula is stabilised
+const double diff_theta = max_theta - min_theta;        // Difference between max and min angle
+const double min_servo_pwm = 0.0021;                    // Corresponds to angle of theta -38 degrees
+const double max_servo_pwm = 0.0024;                    // Corresponds to angle of theta -24 degrees
+const double res_servo = max_servo_pwm - min_servo_pwm; // Resolution of servo pwm ... 
+                                                        // ... signal between min and max angle
 
 // Define the Biquad chains
 BiQuadChain bqc11;
@@ -96,7 +98,7 @@
 BiQuadChain bqc31;
 BiQuadChain bqc32;
 
-// Define the BiQuads for the filter of the first emg signal
+// Define the BiQuads for the filter of the first EMG signal
 // Notch filter
 BiQuad bq111(0.9795,   -1.5849,    0.9795,    1.0000,   -1.5849,    0.9589);
 BiQuad bq112(0.9833,   -1.5912,    0.9833,    1.0000,   -1.5793,    0.9787);
@@ -108,7 +110,7 @@
 // Low pass filter
 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
 
-// Define the BiQuads for the filter of the second emg signal
+// Define the BiQuads for the filter of the second EMG signal
 // Notch filter
 BiQuad bq211 = bq111;
 BiQuad bq212 = bq112;
@@ -120,7 +122,7 @@
 // Low pass filter
 BiQuad bq231 = bq131;
 
-// Define the BiQuads for the filter of the third emg signal
+// Define the BiQuads for the filter of the third EMG signal
 // Notch filter
 BiQuad bq311 = bq111;
 BiQuad bq312 = bq112;
@@ -137,7 +139,7 @@
     if (controller_go == true) {
         // This if statement is used to see if the code takes too long before it is called again
         pc.printf("rate too high, error in activate_controller\n\r");
-        
+
     }
     controller_go = true; // Activate go flag
 }
@@ -151,14 +153,14 @@
     servo_go = true;  // Activate go flag
 }
 
-void sample()   // Function for sampling emg signal and changing reference position
+void sample()   // Function for sampling the EMG signal and changing the reference position
 {
     // Change key if the keyboard is pressed
     if (pc.readable() == 1) {
         key=pc.getc();
     }
-    
-    // Read the emg signals and filter it
+
+    // Read the EMG signals and filter it
     emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read())));    //filtered signal 1
     emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read())));    //filtered signal 2
     emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read())));    //filtered signal 3
@@ -166,32 +168,26 @@
     // Remember what the old reference was
     old_ref_x = ref_x;
     old_ref_y = ref_y;
-    
-    // Change the reference position if emg signals exceed threshold value or if key is pressed
-    if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd')  // Negative XY direction
-    {
+
+    // Change the reference position if the EMG signals exceed the threshold value or if a key is pressed
+    if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd') { // Negative XY direction
         ref_x = ref_x - speed;
         ref_y = ref_y - speed;
 
-    } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z')  // Negative X direction  
-    {
+    } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z') { // Negative X direction
         ref_x = ref_x - speed;
-        
-    } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's')                // Negative Y direction 
-    {
+
+    } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's') {               // Negative Y direction
         ref_y = ref_y - speed;
 
-    } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' )               // Positive XY direction 
-    {
+    } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' ) {              // Positive XY direction
         ref_x = ref_x + speed;
         ref_y = ref_y + speed;
 
-    } else if (emg2 > threshold2 || key == 'q' )                                    // Positive X direction
-    {
+    } else if (emg2 > threshold2 || key == 'q' ) {                                   // Positive X direction
         ref_x = ref_x + speed;
 
-    } else if (emg3 > threshold3 || key == 'w')                                     // Positive Y direction
-    {
+    } else if (emg3 > threshold3 || key == 'w') {                                    // Positive Y direction
         ref_y = ref_y + speed;
     }
 
@@ -213,7 +209,8 @@
     theta = atan( ref_y / (ref_x + min_radius));
     radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
 
-    // If the new reference is outside the possible range then revert back to the old reference unless z is pressed
+    // If the new reference is outside the possible range
+    // then revert back to the old reference unless z is pressed
     if (radius < min_radius) {
         if (key != 'z') {
             ref_x = old_ref_x;
@@ -225,23 +222,25 @@
     } else if (ref_y < min_y) {
         ref_y = old_ref_y;
     }
-    
+
     // Calculate theta and radius again
     theta = atan( ref_y / (ref_x + min_radius));
     radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
 }
 
 double PID( double err, const double Kp, const double Ki, const double Kd,
-            const double Ts, const double N, double &v1, double &v2 )           //discrete PIDF controller (tustin approximation)
+            const double Ts, const double N, double &v1, double &v2 )           
+            //discrete PIDF controller (tustin approximation)
 {
-    const double a1 = -4 / (N * Ts + 2),
-                 a2 = -(N * Ts - 2)/(N*Ts + 2),
-                 b0 = (4 * Kp + 4 * Kd * N + 2 * Ki * Ts + 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4),
-                 b1 = (Ki * N * pow(Ts, 2) - 4 * Kp - 4 * Kd * N) / (N * Ts + 2),
-                 b2 = (4 * Kp + 4 * Kd * N - 2 * Ki * Ts - 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4); //calculate controller coefficients
+    //calculate the controller coefficients
+    const double a1 = -4/(N*Ts+2),
+                 a2 = -(N*Ts-2)/(N*Ts+2),
+                 b0 = (4*Kp+4*Kd*N+2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
+                 b1 = (Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
+                 b2 = (4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
 
-    double v = err - a1 * v1 - a2 * v2;
-    double u = b0 * v + b1 * v1 + b2 * v2;          //input for motors
+    double v = err-a1*v1-a2*v2;
+    double u = b0*v+b1*v1+b2*v2;          //input for motors
     v2 = v1;            //store old value
     v1 = v;             //store old value
     return u;
@@ -253,11 +252,11 @@
     double ref_angle1 = 16 * theta;
     double ref_angle2 = (-radius + min_radius) / pulley_radius;
 
-    // Get number of pulses of the encoders
+    // calculate angle of the encoders
     double angle1 = encoder1.getPulses() / res;   //counterclockwise is positive
     double angle2 = encoder2.getPulses() / res;
-    
-    // Calculate the motor pwm using the function PID() and the voltage
+
+    // Calculate the motor pwm using the function PID() and the maximum voltage
     m1_pwm = (PID(ref_angle1 - angle1, m1_Kp, m1_Ki, m1_Kd, Ts, m1_N, m1_v1, m1_v2)) / V_max;
     m2_pwm = (PID(ref_angle2 - angle2, m2_Kp, m2_Ki, m2_Kd, Ts, m2_N, m2_v1, m2_v2)) / V_max;
 
@@ -281,13 +280,13 @@
 
 void servo_controller()  // Function for stabilizing the spatula with the servo
 {
-    // If theta is smaller than max_theta, servo_pwm is adjusted to stabilise spatula 
-    if (theta < max_theta ) { 
+    // If theta is smaller than max_theta, servo_pwm is adjusted to stabilise spatula
+    if (theta < max_theta ) {
         servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
     } else {
         servo_pwm = max_servo_pwm;
     }
-    
+
     // Spatula goes to its maximum position to flip a burger if button is pressed
     if (!servo_button) {
         servo_pwm = 0.0014;
@@ -297,13 +296,14 @@
     servo.pulsewidth(servo_pwm);
 }
 
-void my_emg()   // This function prints the highest emg values 
+void my_emg()   // This function prints the highest EMG values
 {
-    pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
+    pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r",
+    highest_emg1, highest_emg2, highest_emg3);
 }
 
 
-void my_pos()   // This function prints the reference position 
+void my_pos()   // This function prints the reference position
 {
     pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
 }
@@ -334,7 +334,7 @@
     // Attaching the function my_emg() to the ticker print_timer
     print_timer.attach(&my_emg, 1);
 
-    // While loop used for calibrating emg thresholds, since emg values of muscles differ
+    // While loop used for calibrating the EMG thresholds, since EMG values of muscles differ
     while (servo_button == 1) {
         emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read())));    //filtered signal 1
         emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read())));    //filtered signal 2
@@ -353,7 +353,7 @@
             highest_emg3 = emg3;
         }
 
-        // Define the thresholds as 0.3 times the highest emg value
+        // Define the thresholds as 0.3 times the highest EMG value
         threshold1 = 0.30 * highest_emg1;
         threshold2 = 0.30 * highest_emg2;
         threshold3 = 0.30 * highest_emg3;