Rick Poppe
code die in het verslag komt
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Revision 9:ef6c5feb5623, committed 2016-11-07
- 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;