Revision 5:0581d843fde2, committed 2016-11-05

Comitter:

RiP

Date:

Sat Nov 05 17:27:31 2016 +0000

Parent:

4:b83460036800

Commit message:

overal goed commentaar geleverd behalve bij de functie PID(); ook zijn sample() en motor_controller samengevoegd in 1 go flag

Changed in this revision

--- a/main.cpp	Sat Nov 05 16:40:39 2016 +0000
+++ b/main.cpp	Sat Nov 05 17:27:31 2016 +0000
@@ -8,7 +8,7 @@
 // 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)
 
-MODSERIAL pc(USBTX, USBRX);
+MODSERIAL   pc(USBTX, USBRX);
 
 // Define the EMG inputs
 AnalogIn    emg_in1( A0 );
@@ -16,72 +16,60 @@
 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
+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
 
-// Define servo input
-DigitalIn servo_button(PTC12);
+// 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);
+QEI         encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
 
 // Define the Tickers
 Ticker      print_timer;                                                // Ticker for printing the position or highest EMG values
-Ticker      sample_timer;                                               // Ticker for when a sample needs to be taken
-Ticker      control_timer;                                              // Ticker for processing encoder input to motor output
+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
 
 // Define the Time constants
-const double freq     = 0.002;                                          // EMG sample time
-const double m1_Ts    = 0.002;                                          // Controller sample time
-const double m2_Ts    = 0.002;                                          // Controller sample time
-const double servo_Ts =  0.02;                                          // Servo controller sample time
+const double  Ts = 0.002;                                               // Time constant for activate_controller()
+const double  servo_Ts = 0.02;                                          // Time constant for activate_servo_controller()
 
 // Define the go flags
-volatile bool change_ref_go     =   false;                                  // Go flag sample()
-volatile bool controller_go =   false;                                  // Go flag controller()
-volatile bool servo_go      =   false;                                  // Go flag servo_controller()
+volatile bool change_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;                                                            // The first EMG signal
-double emg2;                                                            // The second EMG signal
-double emg3;                                                            // The third EMG signal
-double highest_emg1;                                                    // The highest EMG signal of emg_in1
-double highest_emg2;                                                    // The highest EMG signal of emg_in2
-double highest_emg3;                                                    // The highest EMG signal of emg_in3
-double threshold1;                                                      // The threshold which the first EMG signal needs to surpass to do something
-double threshold2;                                                      // The threshold which the second EMG signal needs to surpass to do something
-double threshold3;                                                      // The threshold which the third EMG signal needs to surpass to do something
+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
 
 //Define the keyboard input
 char key;                                                               // Stores the last pressed key on the keyboard
 
 // Define the reference variables
-double ref_x = 0.0;                                                     // The x reference position
-double ref_y = 0.0;                                                     // The y reference position
-double old_ref_x;                                                       // The old x reference
-double old_ref_y;                                                       // The old y reference
+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
 bool   z_pushed = false;                                                // To see if z is pressed
 
 // Define reference limits
-const double min_radius=0.43;                                           // The minimum radius of arm
-const double max_radius=0.62;                                           // The maximum radius of arm
-const double min_y=-0.26;                                               // The minimum height which the spatula can reach
+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
 
 // Define variables of motor 1
-double m1_pwm=0;                                                        // Variable for PWM control 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_v1 = 0, m1_v2 = 0;                                            // Memory variables
 
 // Define variables of motor 2
-double m2_pwm=0;                                                        // Variable for PWM control 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_v1 = 0, m2_v2 = 0;                                            // Memory variables
 
@@ -144,158 +132,148 @@
 // Low pass filter
 BiQuad bq331 = bq131;
 
-void activate_sample() // Go flag for the function sample()
+void activate_controller() // Go flag for the functions sample() and controller()
 {
-    if (change_ref_go==true) {
+    if (change_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_sample\n\r");
+        pc.printf("rate too high, error in activate_controller\n\r");
     }
-    change_ref_go=true;   // Activate go flag
-}
-
-void activate_controller()  // Go flag for the function activate_controller()
-{
-    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
+    change_controller_go = true; // Activate go flag
 }
 
 void activate_servo_control()   // Go flag for the function servo_controller()
 {
-    if (servo_go==true) {
+    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 servo_controller()\n\r");
     }
-    servo_go=true;   // Activate go flag
+    servo_go = true;  // Activate go flag
 }
 
-void change_ref()   // Function for sampling of the emg signal and changing the reference position
+void sample()   // Function for sampling of the emg signal and changing the reference position
 {
     // Change key if the keyboard is pressed
-    if (pc.readable()==1) {
+    if (pc.readable() == 1) {
         key=pc.getc();
     }
     
     // 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
+    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
 
     // Remember what the old reference was
-    old_ref_x=ref_x;
-    old_ref_y=ref_y;
+    old_ref_x = ref_x;
+    old_ref_y = ref_y;
     
     // Change the reference if the emg signals go over the threshold
-    if (emg1>threshold1&&emg2>threshold2&&emg3>threshold3 || key=='d')          // Negative XY direction
+    if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd')  // Negative XY direction
     {
-        ref_x=ref_x-speed;
-        ref_y=ref_y-speed;
+        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;
+        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;
+        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;
+        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;
+        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;
+        ref_y = ref_y + speed;
     }
 
     // Change z_pushed to true if 'z' is pressed on the keyboard
     if (key == 'z') {
-        z_pushed=true;
+        z_pushed = true;
     }
 
     // Reset the reference and the encoders if z is no longer pressed
     if (key != 'z' && z_pushed) {
-        ref_x=0.0;
-        ref_y=0.0;
-        Encoder1.reset();
-        Encoder2.reset();
-        z_pushed=false;
+        ref_x = 0.0;
+        ref_y = 0.0;
+        encoder1.reset();
+        encoder2.reset();
+        z_pushed = false;
     }
 
     // Convert the x and y reference to the theta and radius reference
-    theta=atan(ref_y/(ref_x+min_radius));
-    radius=sqrt(pow(ref_x+min_radius,2)+pow(ref_y,2));
+    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 (radius < min_radius) {
         if (key != 'z') {
-            ref_x=old_ref_x;
-            ref_y=old_ref_y;
+            ref_x = old_ref_x;
+            ref_y = old_ref_y;
         }
     } else if ( radius > max_radius) {
-        ref_x=old_ref_x;
-        ref_y=old_ref_y;
-    } else if (ref_y<min_y) {
-        ref_y=old_ref_y;
+        ref_x = old_ref_x;
+        ref_y = old_ref_y;
+    } 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));
-
+    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 filter
 {
-    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);
+    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;
-    v2=v1;
-    v1=v;
+    double v = err - a1 * v1 - a2 * v2;
+    double u = b0 * v + b1 * v1 + b2 * v2;
+    v2 = v1;
+    v1 = v;
     return u;
 }
 
-void controller()  // Function for executing controller action
+void motor_controller()  // Function for executing controller action
 {
     // Convert radius and theta to gearbox angles
-    double ref_angle1=16*theta;
-    double ref_angle2=(-radius+min_radius)/pulley_radius;
+    double ref_angle1 = 16 * theta;
+    double ref_angle2 = (-radius + min_radius) / pulley_radius;
 
     // Get number of pulses of the encoders
-    double angle1 = Encoder1.getPulses()/res;   //counterclockwise is positive
-    double angle2 = Encoder2.getPulses()/res;
+    double angle1 = encoder1.getPulses() / res;   //counterclockwise is positive
+    double angle2 = encoder2.getPulses() / res;
     
     // Calculate the motor pwm using the function PID() and the voltage
-    m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
-    m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
+    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;
 
     // Limit pwm value and change motor direction when pwm becomes either negative or positive
-    if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
-        motor1dir=0;
+    if (m1_pwm >= 0.0f && m1_pwm <= 1.0f) {
+        motor1dir = 0;
         motor1.write(m1_pwm);
     } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
-        motor1dir=1;
+        motor1dir = 1;
         motor1.write(-m1_pwm);
     }
 
-    if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
-        motor2dir=0;
+    if (m2_pwm >= 0.0f && m2_pwm <= 1.0f) {
+        motor2dir = 0;
         motor2.write(m2_pwm);
     } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
-        motor2dir=1;
+        motor2dir = 1;
         motor2.write(-m2_pwm);
     }
 }
@@ -304,19 +282,18 @@
 {
     // 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
-        servo_pwm=min_servo_pwm+(theta-min_theta)/diff_theta*res_servo;
+        servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
     } else {
-        servo_pwm=max_servo_pwm;
+        servo_pwm = max_servo_pwm;
     }
     
     // The spatula goes to its maximum position to flip a burger if the button is pressed
     if (!servo_button) {
-        servo_pwm=0.0014;
+        servo_pwm = 0.0014;
     }
 
     // Send the servo_pwm to the servo
     servo.pulsewidth(servo_pwm);
-
 }
 
 void my_emg()   // This function prints the highest emg values to putty
@@ -350,64 +327,56 @@
     motor2.period(0.02f);
 
     // Set the direction of the motors to ccw
-    motor1dir=0;
-    motor2dir=0;
+    motor1dir = 0;
+    motor2dir = 0;
 
     // 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 (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
-        emg3=bqc32.step(fabs(bqc31.step(emg_in3.read())));    //filtered signal 3
+    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
+        emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read())));    //filtered signal 3
 
         // Check if the new EMG signal is higher than the current highest value
-        if(emg1>highest_emg1) {
-            highest_emg1=emg1;
+        if(emg1 > highest_emg1) {
+            highest_emg1 = emg1;
         }
 
-        if(emg2>highest_emg2) {
-            highest_emg2=emg2;
+        if(emg2 > highest_emg2) {
+            highest_emg2 = emg2;
         }
 
-        if(emg3>highest_emg3) {
-            highest_emg3=emg3;
+        if(emg3 > highest_emg3) {
+            highest_emg3 = emg3;
         }
 
-        // Define the thresholds as 0.3 the highest emg value
-        threshold1=0.30*highest_emg1;
-        threshold2=0.30*highest_emg2;
-        threshold3=0.30*highest_emg3;
+        // 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;
     }
 
-    // Attach the function sample() to the ticker sample_timer
-    sample_timer.attach(&activate_sample, freq);
+    // Attach the function activate_controller() to the ticker change_pos_timer
+    controller_timer.attach(&activate_controller, Ts);
 
     // Attach the function my_pos() to the ticker print_timer.
     print_timer.attach(&my_pos, 1);
 
-    // Attach the function activate_controller() to the ticker control_timer
-    control_timer.attach(&activate_controller,m1_Ts);
-
     // Attach the function activate_servo_control() to the ticker servo_timer
     servo_timer.attach(&activate_servo_control,servo_Ts);
 
     while(1) {
-        // Only take a sample when the go flag is true.
-        if (change_ref_go==true) {
-            change_ref();
-            change_ref_go = false;
+        // Take a sample and control the motor when the go flag is true.
+        if (change_controller_go == true) {
+            sample();
+            motor_controller();
+            change_controller_go = false;
         }
 
-        // Only control the motor when the go flag is true
-        if(controller_go) {
-            controller();
-            controller_go=false;
-        }
-
-        // Only control the servo when the go flag is true
-        if(servo_go) {
+        // Control the servo when the go flag is true
+        if(servo_go == true) {
             servo_controller();
             servo_go=false;
         }