Revision 6:2225bdcfcd6e, committed 2016-11-06

Comitter:

mefix

Date:

Sun Nov 06 11:38:23 2016 +0000

Parent:

5:0581d843fde2

Child:

7:bb51e2421953

Commit message:

added and changed several comments

Changed in this revision

--- a/main.cpp	Sat Nov 05 17:27:31 2016 +0000
+++ b/main.cpp	Sun Nov 06 11:38:23 2016 +0000
@@ -6,71 +6,71 @@
 #include "FastPWM.h"
 
 // In use: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
-// 8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) 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 );
+AnalogIn    emg_in1( A0 );  // welke spieren?
 AnalogIn    emg_in2( A1 );
 AnalogIn    emg_in3( A2 );
 
 // 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);                                                  // Speed of motor 1
-FastPWM     motor2(D5);                                                  // Speed of motor 2
-FastPWM     servo(D9);                                                   // Servo pwm
+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);
-QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
+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 the position or highest EMG values
-Ticker      controller_timer;                                           // Ticker for when a sample needs to be taken and the motor need to be controlled
-Ticker      servo_timer;                                                // Ticker for calling 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 activate_controller()
-const double  servo_Ts = 0.02;                                          // Time constant for activate_servo_controller()
+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 change_controller_go = false;                             // Go flag for sample() and motor_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 which the EMG signals need to surpass to change the reference
+double threshold1, threshold2, threshold3;                              // The threshold for the EMG signals to change the reference
 
 //Define the keyboard input
-char key;                                                               // Stores the last pressed key on the keyboard
+char key;                                                               // Stores last pressed key
 
 // Define the reference variables
-double ref_x = 0.0, ref_y = 0.0;                                        // The reference position
-double old_ref_x, old_ref_y;                                            // The old reference position
-double speed = 0.00006;                                                 // The variable with which a speed is reached of 3 cm/s in x and y direction
-double theta = 0.0;                                                     // The angle reference of the arm
-double radius = 0.0;                                                    // The radius reference of the arm
+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 height which the spatula can reach
+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 control motor 1
-const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100;    // PID values of 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
 
 // Define variables of motor 2
-double m2_pwm = 0;                                                      // Variable for PWM control motor 2
-const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100;   // PID values of 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
 
 // Define machine constants
@@ -80,10 +80,10 @@
 const double pulley_radius = 0.0398/2.0;                                // Pulley radius
 
 // Define variables needed for controlling the servo
-double       servo_pwm = 0.0023;                                        // Duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
-const double min_theta = -37.0 / 180.0 * pi;                            // Minimum angle robot
-const double max_theta = -14.0 / 180.0 * pi;                            // Maximum angle to which the spatula can stabilise
-const double diff_theta = max_theta - min_theta;                        // Difference between max and min angle of theta for stabilisation
+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
@@ -132,25 +132,26 @@
 // Low pass filter
 BiQuad bq331 = bq131;
 
-void activate_controller() // Go flag for the functions sample() and controller()
+void activate_controller() // Go flag for sample() and controller()
 {
-    if (change_controller_go == true) {
+    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");
+        
     }
-    change_controller_go = true; // Activate go flag
+    controller_go = true; // Activate go flag
 }
 
-void activate_servo_control()   // Go flag for the function servo_controller()
+void activate_servo_control()   // Go flag for servo_controller()
 {
     if (servo_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 servo_controller()\n\r");
+        pc.printf("rate too high, error in activate_servo_control()\n\r");
     }
     servo_go = true;  // Activate go flag
 }
 
-void sample()   // Function for sampling of the emg signal and changing the reference position
+void sample()   // Function for sampling emg signal and changing reference position
 {
     // Change key if the keyboard is pressed
     if (pc.readable() == 1) {
@@ -166,7 +167,7 @@
     old_ref_x = ref_x;
     old_ref_y = ref_y;
     
-    // Change the reference if the emg signals go over the threshold
+    // 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
     {
         ref_x = ref_x - speed;
@@ -231,18 +232,18 @@
 }
 
 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 filter
+            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);
+                 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
 
     double v = err - a1 * v1 - a2 * v2;
-    double u = b0 * v + b1 * v1 + b2 * v2;
-    v2 = v1;
-    v1 = v;
+    double u = b0 * v + b1 * v1 + b2 * v2;          //input for motors
+    v2 = v1;            //store old value
+    v1 = v;             //store old value
     return u;
 }
 
@@ -278,16 +279,16 @@
     }
 }
 
-void servo_controller()  // Function  for stabilizing the spatula with the servo
+void servo_controller()  // Function for stabilizing the spatula with the servo
 {
-    // If theta is smaller than max_theta then the servo_pwm is adjusted to stabilize the spatula 
-    if (theta < max_theta ) { // servo can stabilize until maximum 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;
     }
     
-    // The spatula goes to its maximum position to flip a burger if the button is pressed
+    // Spatula goes to its maximum position to flip a burger if button is pressed
     if (!servo_button) {
         servo_pwm = 0.0014;
     }
@@ -296,13 +297,13 @@
     servo.pulsewidth(servo_pwm);
 }
 
-void my_emg()   // This function prints the highest emg values to putty
+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);
 }
 
 
-void my_pos()   // This function prints the reference position to putty
+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);
 }
@@ -333,7 +334,7 @@
     // Attaching the function my_emg() to the ticker print_timer
     print_timer.attach(&my_emg, 1);
 
-    // While loop used for calibrating the emg thresholds, So that every user can use it
+    // While loop used for calibrating 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
@@ -369,10 +370,10 @@
 
     while(1) {
         // Take a sample and control the motor when the go flag is true.
-        if (change_controller_go == true) {
+        if (controller_go == true) {
             sample();
             motor_controller();
-            change_controller_go = false;
+            controller_go = false;
         }
 
         // Control the servo when the go flag is true