Rick Poppe / Mbed 2 deprecated final_control_v3

naam van een functie veranderd

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@5:0581d843fde2, 2016-11-05 (annotated)

Committer:
RiP
Date:
Sat Nov 05 17:27:31 2016 +0000
Revision:
5:0581d843fde2
Parent:
4:b83460036800 
overal goed commentaar geleverd behalve bij de functie PID(); ook zijn sample() en motor_controller samengevoegd in 1 go flag

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mefix 0:3c99f1705565 1 #include "mbed.h"
mefix 0:3c99f1705565 2 #include "HIDScope.h"
mefix 0:3c99f1705565 3 #include "BiQuad.h"
mefix 0:3c99f1705565 4 #include "MODSERIAL.h"
mefix 0:3c99f1705565 5 #include "QEI.h"
mefix 0:3c99f1705565 6 #include "FastPWM.h"
mefix 0:3c99f1705565 7
RiP 4:b83460036800 8 // In use: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
RiP 4:b83460036800 9 // 8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) A(0,1,2)(EMG)
mefix 0:3c99f1705565 10
RiP 5:0581d843fde2 11 MODSERIAL pc(USBTX, USBRX);
mefix 0:3c99f1705565 12
RiP 4:b83460036800 13 // Define the EMG inputs
RiP 4:b83460036800 14 AnalogIn emg_in1( A0 );
RiP 4:b83460036800 15 AnalogIn emg_in2( A1 );
RiP 4:b83460036800 16 AnalogIn emg_in3( A2 );
mefix 0:3c99f1705565 17
RiP 4:b83460036800 18 // Define motor outputs
RiP 5:0581d843fde2 19 DigitalOut motor1dir(D7); // Direction of motor 1, attach at m1, set to 0: cw
RiP 5:0581d843fde2 20 DigitalOut motor2dir(D4); // Direction of motor 2, attach at m2, set to 0: ccw
RiP 5:0581d843fde2 21 FastPWM motor1(D6); // Speed of motor 1
RiP 5:0581d843fde2 22 FastPWM motor2(D5); // Speed of motor 2
RiP 5:0581d843fde2 23 FastPWM servo(D9); // Servo pwm
RiP 4:b83460036800 24
RiP 5:0581d843fde2 25 // Define button for flipping the spatula
RiP 5:0581d843fde2 26 DigitalIn servo_button(PTC12);
RiP 4:b83460036800 27
RiP 4:b83460036800 28 // Define encoder inputs
RiP 5:0581d843fde2 29 QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
RiP 5:0581d843fde2 30 QEI encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
mefix 0:3c99f1705565 31
RiP 4:b83460036800 32 // Define the Tickers
RiP 4:b83460036800 33 Ticker print_timer; // Ticker for printing the position or highest EMG values
RiP 5:0581d843fde2 34 Ticker controller_timer; // Ticker for when a sample needs to be taken and the motor need to be controlled
RiP 4:b83460036800 35 Ticker servo_timer; // Ticker for calling servo_control
mefix 0:3c99f1705565 36
RiP 4:b83460036800 37 // Define the Time constants
RiP 5:0581d843fde2 38 const double Ts = 0.002; // Time constant for activate_controller()
RiP 5:0581d843fde2 39 const double servo_Ts = 0.02; // Time constant for activate_servo_controller()
mefix 0:3c99f1705565 40
RiP 4:b83460036800 41 // Define the go flags
RiP 5:0581d843fde2 42 volatile bool change_controller_go = false; // Go flag for sample() and motor_controller()
RiP 5:0581d843fde2 43 volatile bool servo_go = false; // Go flag servo_controller()
mefix 0:3c99f1705565 44
RiP 4:b83460036800 45 // Define the EMG variables
RiP 5:0581d843fde2 46 double emg1, emg2, emg3; // The three filtered EMG signals
RiP 5:0581d843fde2 47 double highest_emg1, highest_emg2, highest_emg3; // The highest EMG signals of emg_in
RiP 5:0581d843fde2 48 double threshold1, threshold2, threshold3; // The threshold which the EMG signals need to surpass to change the reference
RiP 1:ba63033da653 49
RiP 4:b83460036800 50 //Define the keyboard input
RiP 4:b83460036800 51 char key; // Stores the last pressed key on the keyboard
mefix 0:3c99f1705565 52
RiP 4:b83460036800 53 // Define the reference variables
RiP 5:0581d843fde2 54 double ref_x = 0.0, ref_y = 0.0; // The reference position
RiP 5:0581d843fde2 55 double old_ref_x, old_ref_y; // The old reference position
RiP 4:b83460036800 56 double speed = 0.00006; // The variable with which a speed is reached of 3 cm/s in x and y direction
RiP 4:b83460036800 57 double theta = 0.0; // The angle reference of the arm
RiP 4:b83460036800 58 double radius = 0.0; // The radius reference of the arm
RiP 4:b83460036800 59 bool z_pushed = false; // To see if z is pressed
mefix 0:3c99f1705565 60
RiP 4:b83460036800 61 // Define reference limits
RiP 5:0581d843fde2 62 const double min_radius = 0.43; // The minimum radius of the arm
RiP 5:0581d843fde2 63 const double max_radius = 0.62; // The maximum radius of the arm
RiP 5:0581d843fde2 64 const double min_y = -0.26; // The minimum height which the spatula can reach
mefix 0:3c99f1705565 65
RiP 4:b83460036800 66 // Define variables of motor 1
RiP 5:0581d843fde2 67 double m1_pwm = 0; // Variable for PWM control motor 1
RiP 4:b83460036800 68 const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // PID values of motor 1
RiP 4:b83460036800 69 double m1_v1 = 0, m1_v2 = 0; // Memory variables
mefix 0:3c99f1705565 70
RiP 4:b83460036800 71 // Define variables of motor 2
RiP 5:0581d843fde2 72 double m2_pwm = 0; // Variable for PWM control motor 2
RiP 4:b83460036800 73 const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // PID values of motor 2
RiP 4:b83460036800 74 double m2_v1 = 0, m2_v2 = 0; // Memory variables
mefix 0:3c99f1705565 75
RiP 4:b83460036800 76 // Define machine constants
RiP 4:b83460036800 77 const double pi = 3.14159265359;
RiP 4:b83460036800 78 const double res = 64.0 / (1.0 / 131.25 * 2.0 * pi); // Resolution on gearbox shaft per pulse
RiP 4:b83460036800 79 const double V_max = 9.0; // Maximum voltage supplied by trafo
RiP 4:b83460036800 80 const double pulley_radius = 0.0398/2.0; // Pulley radius
mefix 0:3c99f1705565 81
RiP 4:b83460036800 82 // Define variables needed for controlling the servo
RiP 4:b83460036800 83 double servo_pwm = 0.0023; // Duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
RiP 4:b83460036800 84 const double min_theta = -37.0 / 180.0 * pi; // Minimum angle robot
RiP 4:b83460036800 85 const double max_theta = -14.0 / 180.0 * pi; // Maximum angle to which the spatula can stabilise
RiP 4:b83460036800 86 const double diff_theta = max_theta - min_theta; // Difference between max and min angle of theta for stabilisation
RiP 4:b83460036800 87 const double min_servo_pwm = 0.0021; // Corresponds to angle of theta -38 degrees
RiP 4:b83460036800 88 const double max_servo_pwm = 0.0024; // Corresponds to angle of theta -24 degrees
RiP 4:b83460036800 89 const double res_servo = max_servo_pwm - min_servo_pwm; // Resolution of servo pwm signal between min and max angle
mefix 0:3c99f1705565 90
RiP 4:b83460036800 91 // Define the Biquad chains
mefix 0:3c99f1705565 92 BiQuadChain bqc11;
RiP 4:b83460036800 93 BiQuadChain bqc12;
mefix 0:3c99f1705565 94 BiQuadChain bqc21;
RiP 4:b83460036800 95 BiQuadChain bqc22;
mefix 0:3c99f1705565 96 BiQuadChain bqc31;
RiP 4:b83460036800 97 BiQuadChain bqc32;
mefix 0:3c99f1705565 98
RiP 4:b83460036800 99 // Define the BiQuads for the filter of the first emg signal
RiP 4:b83460036800 100 // Notch filter
mefix 0:3c99f1705565 101 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
mefix 0:3c99f1705565 102 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
mefix 0:3c99f1705565 103 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 4:b83460036800 104 // High pass filter
mefix 0:3c99f1705565 105 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
mefix 0:3c99f1705565 106 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
mefix 0:3c99f1705565 107 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
RiP 4:b83460036800 108 // Low pass filter
mefix 0:3c99f1705565 109 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
mefix 0:3c99f1705565 110
RiP 4:b83460036800 111 // Define the BiQuads for the filter of the second emg signal
RiP 4:b83460036800 112 // Notch filter
mefix 0:3c99f1705565 113 BiQuad bq211 = bq111;
mefix 0:3c99f1705565 114 BiQuad bq212 = bq112;
mefix 0:3c99f1705565 115 BiQuad bq213 = bq113;
RiP 4:b83460036800 116 // High pass filter
mefix 0:3c99f1705565 117 BiQuad bq221 = bq121;
mefix 0:3c99f1705565 118 BiQuad bq222 = bq122;
mefix 0:3c99f1705565 119 BiQuad bq223 = bq123;
RiP 4:b83460036800 120 // Low pass filter
mefix 0:3c99f1705565 121 BiQuad bq231 = bq131;
mefix 0:3c99f1705565 122
RiP 4:b83460036800 123 // Define the BiQuads for the filter of the third emg signal
RiP 4:b83460036800 124 // Notch filter
mefix 0:3c99f1705565 125 BiQuad bq311 = bq111;
mefix 0:3c99f1705565 126 BiQuad bq312 = bq112;
mefix 0:3c99f1705565 127 BiQuad bq313 = bq113;
RiP 4:b83460036800 128 // High pass filter
mefix 0:3c99f1705565 129 BiQuad bq321 = bq121;
mefix 0:3c99f1705565 130 BiQuad bq323 = bq122;
mefix 0:3c99f1705565 131 BiQuad bq322 = bq123;
RiP 4:b83460036800 132 // Low pass filter
mefix 0:3c99f1705565 133 BiQuad bq331 = bq131;
mefix 0:3c99f1705565 134
RiP 5:0581d843fde2 135 void activate_controller() // Go flag for the functions sample() and controller()
mefix 0:3c99f1705565 136 {
RiP 5:0581d843fde2 137 if (change_controller_go == true) {
RiP 4:b83460036800 138 // This if statement is used to see if the code takes too long before it is called again
RiP 5:0581d843fde2 139 pc.printf("rate too high, error in activate_controller\n\r");
mefix 0:3c99f1705565 140 }
RiP 5:0581d843fde2 141 change_controller_go = true; // Activate go flag
mefix 0:3c99f1705565 142 }
mefix 0:3c99f1705565 143
RiP 4:b83460036800 144 void activate_servo_control() // Go flag for the function servo_controller()
mefix 0:3c99f1705565 145 {
RiP 5:0581d843fde2 146 if (servo_go == true) {
RiP 4:b83460036800 147 // This if statement is used to see if the code takes too long before it is called again
RiP 5:0581d843fde2 148 pc.printf("rate too high, error in servo_controller()\n\r");
mefix 0:3c99f1705565 149 }
RiP 5:0581d843fde2 150 servo_go = true; // Activate go flag
mefix 0:3c99f1705565 151 }
mefix 0:3c99f1705565 152
RiP 5:0581d843fde2 153 void sample() // Function for sampling of the emg signal and changing the reference position
mefix 0:3c99f1705565 154 {
RiP 4:b83460036800 155 // Change key if the keyboard is pressed
RiP 5:0581d843fde2 156 if (pc.readable() == 1) {
mefix 0:3c99f1705565 157 key=pc.getc();
mefix 0:3c99f1705565 158 }
RiP 4:b83460036800 159
RiP 4:b83460036800 160 // Read the emg signals and filter it
RiP 5:0581d843fde2 161 emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
RiP 5:0581d843fde2 162 emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
RiP 5:0581d843fde2 163 emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
mefix 0:3c99f1705565 164
RiP 4:b83460036800 165 // Remember what the old reference was
RiP 5:0581d843fde2 166 old_ref_x = ref_x;
RiP 5:0581d843fde2 167 old_ref_y = ref_y;
RiP 4:b83460036800 168
RiP 4:b83460036800 169 // Change the reference if the emg signals go over the threshold
RiP 5:0581d843fde2 170 if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd') // Negative XY direction
RiP 4:b83460036800 171 {
RiP 5:0581d843fde2 172 ref_x = ref_x - speed;
RiP 5:0581d843fde2 173 ref_y = ref_y - speed;
mefix 0:3c99f1705565 174
RiP 5:0581d843fde2 175 } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z') // Negative X direction
RiP 4:b83460036800 176 {
RiP 5:0581d843fde2 177 ref_x = ref_x - speed;
RiP 4:b83460036800 178
RiP 5:0581d843fde2 179 } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's') // Negative Y direction
RiP 4:b83460036800 180 {
RiP 5:0581d843fde2 181 ref_y = ref_y - speed;
mefix 0:3c99f1705565 182
RiP 5:0581d843fde2 183 } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' ) // Positive XY direction
RiP 4:b83460036800 184 {
RiP 5:0581d843fde2 185 ref_x = ref_x + speed;
RiP 5:0581d843fde2 186 ref_y = ref_y + speed;
mefix 0:3c99f1705565 187
RiP 5:0581d843fde2 188 } else if (emg2 > threshold2 || key == 'q' ) // Positive X direction
RiP 4:b83460036800 189 {
RiP 5:0581d843fde2 190 ref_x = ref_x + speed;
mefix 0:3c99f1705565 191
RiP 5:0581d843fde2 192 } else if (emg3 > threshold3 || key == 'w') // Positive Y direction
RiP 4:b83460036800 193 {
RiP 5:0581d843fde2 194 ref_y = ref_y + speed;
mefix 0:3c99f1705565 195 }
mefix 0:3c99f1705565 196
RiP 4:b83460036800 197 // Change z_pushed to true if 'z' is pressed on the keyboard
RiP 3:58378b78079d 198 if (key == 'z') {
RiP 5:0581d843fde2 199 z_pushed = true;
RiP 3:58378b78079d 200 }
RiP 3:58378b78079d 201
RiP 4:b83460036800 202 // Reset the reference and the encoders if z is no longer pressed
RiP 4:b83460036800 203 if (key != 'z' && z_pushed) {
RiP 5:0581d843fde2 204 ref_x = 0.0;
RiP 5:0581d843fde2 205 ref_y = 0.0;
RiP 5:0581d843fde2 206 encoder1.reset();
RiP 5:0581d843fde2 207 encoder2.reset();
RiP 5:0581d843fde2 208 z_pushed = false;
mefix 0:3c99f1705565 209 }
mefix 0:3c99f1705565 210
RiP 4:b83460036800 211 // Convert the x and y reference to the theta and radius reference
RiP 5:0581d843fde2 212 theta = atan( ref_y / (ref_x + min_radius));
RiP 5:0581d843fde2 213 radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
mefix 0:3c99f1705565 214
RiP 4:b83460036800 215 // If the new reference is outside the possible range then revert back to the old reference unless z is pressed
RiP 4:b83460036800 216 if (radius < min_radius) {
mefix 0:3c99f1705565 217 if (key != 'z') {
RiP 5:0581d843fde2 218 ref_x = old_ref_x;
RiP 5:0581d843fde2 219 ref_y = old_ref_y;
mefix 0:3c99f1705565 220 }
RiP 4:b83460036800 221 } else if ( radius > max_radius) {
RiP 5:0581d843fde2 222 ref_x = old_ref_x;
RiP 5:0581d843fde2 223 ref_y = old_ref_y;
RiP 5:0581d843fde2 224 } else if (ref_y < min_y) {
RiP 5:0581d843fde2 225 ref_y = old_ref_y;
mefix 0:3c99f1705565 226 }
RiP 4:b83460036800 227
RiP 4:b83460036800 228 // Calculate theta and radius again
RiP 5:0581d843fde2 229 theta = atan( ref_y / (ref_x + min_radius));
RiP 5:0581d843fde2 230 radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
mefix 0:3c99f1705565 231 }
mefix 0:3c99f1705565 232
mefix 0:3c99f1705565 233 double PID( double err, const double Kp, const double Ki, const double Kd,
RiP 4:b83460036800 234 const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
mefix 0:3c99f1705565 235 {
RiP 5:0581d843fde2 236 const double a1 = -4 / (N * Ts + 2),
RiP 5:0581d843fde2 237 a2 = -(N * Ts - 2)/(N*Ts + 2),
RiP 5:0581d843fde2 238 b0 = (4 * Kp + 4 * Kd * N + 2 * Ki * Ts + 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4),
RiP 5:0581d843fde2 239 b1 = (Ki * N * pow(Ts, 2) - 4 * Kp - 4 * Kd * N) / (N * Ts + 2),
RiP 5:0581d843fde2 240 b2 = (4 * Kp + 4 * Kd * N - 2 * Ki * Ts - 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4);
mefix 0:3c99f1705565 241
RiP 5:0581d843fde2 242 double v = err - a1 * v1 - a2 * v2;
RiP 5:0581d843fde2 243 double u = b0 * v + b1 * v1 + b2 * v2;
RiP 5:0581d843fde2 244 v2 = v1;
RiP 5:0581d843fde2 245 v1 = v;
mefix 0:3c99f1705565 246 return u;
mefix 0:3c99f1705565 247 }
mefix 0:3c99f1705565 248
RiP 5:0581d843fde2 249 void motor_controller() // Function for executing controller action
mefix 0:3c99f1705565 250 {
RiP 4:b83460036800 251 // Convert radius and theta to gearbox angles
RiP 5:0581d843fde2 252 double ref_angle1 = 16 * theta;
RiP 5:0581d843fde2 253 double ref_angle2 = (-radius + min_radius) / pulley_radius;
mefix 0:3c99f1705565 254
RiP 4:b83460036800 255 // Get number of pulses of the encoders
RiP 5:0581d843fde2 256 double angle1 = encoder1.getPulses() / res; //counterclockwise is positive
RiP 5:0581d843fde2 257 double angle2 = encoder2.getPulses() / res;
RiP 4:b83460036800 258
RiP 4:b83460036800 259 // Calculate the motor pwm using the function PID() and the voltage
RiP 5:0581d843fde2 260 m1_pwm = (PID(ref_angle1 - angle1, m1_Kp, m1_Ki, m1_Kd, Ts, m1_N, m1_v1, m1_v2)) / V_max;
RiP 5:0581d843fde2 261 m2_pwm = (PID(ref_angle2 - angle2, m2_Kp, m2_Ki, m2_Kd, Ts, m2_N, m2_v1, m2_v2)) / V_max;
mefix 0:3c99f1705565 262
RiP 4:b83460036800 263 // Limit pwm value and change motor direction when pwm becomes either negative or positive
RiP 5:0581d843fde2 264 if (m1_pwm >= 0.0f && m1_pwm <= 1.0f) {
RiP 5:0581d843fde2 265 motor1dir = 0;
mefix 0:3c99f1705565 266 motor1.write(m1_pwm);
mefix 0:3c99f1705565 267 } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
RiP 5:0581d843fde2 268 motor1dir = 1;
mefix 0:3c99f1705565 269 motor1.write(-m1_pwm);
mefix 0:3c99f1705565 270 }
mefix 0:3c99f1705565 271
RiP 5:0581d843fde2 272 if (m2_pwm >= 0.0f && m2_pwm <= 1.0f) {
RiP 5:0581d843fde2 273 motor2dir = 0;
mefix 0:3c99f1705565 274 motor2.write(m2_pwm);
mefix 0:3c99f1705565 275 } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
RiP 5:0581d843fde2 276 motor2dir = 1;
mefix 0:3c99f1705565 277 motor2.write(-m2_pwm);
mefix 0:3c99f1705565 278 }
mefix 0:3c99f1705565 279 }
mefix 0:3c99f1705565 280
RiP 4:b83460036800 281 void servo_controller() // Function for stabilizing the spatula with the servo
mefix 0:3c99f1705565 282 {
RiP 4:b83460036800 283 // If theta is smaller than max_theta then the servo_pwm is adjusted to stabilize the spatula
RiP 4:b83460036800 284 if (theta < max_theta ) { // servo can stabilize until maximum theta
RiP 5:0581d843fde2 285 servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
mefix 0:3c99f1705565 286 } else {
RiP 5:0581d843fde2 287 servo_pwm = max_servo_pwm;
mefix 0:3c99f1705565 288 }
RiP 4:b83460036800 289
RiP 4:b83460036800 290 // The spatula goes to its maximum position to flip a burger if the button is pressed
RiP 4:b83460036800 291 if (!servo_button) {
RiP 5:0581d843fde2 292 servo_pwm = 0.0014;
RiP 3:58378b78079d 293 }
RiP 3:58378b78079d 294
RiP 4:b83460036800 295 // Send the servo_pwm to the servo
mefix 0:3c99f1705565 296 servo.pulsewidth(servo_pwm);
mefix 0:3c99f1705565 297 }
mefix 0:3c99f1705565 298
RiP 4:b83460036800 299 void my_emg() // This function prints the highest emg values to putty
RiP 1:ba63033da653 300 {
RiP 1:ba63033da653 301 pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
RiP 1:ba63033da653 302 }
RiP 1:ba63033da653 303
mefix 0:3c99f1705565 304
RiP 4:b83460036800 305 void my_pos() // This function prints the reference position to putty
mefix 0:3c99f1705565 306 {
mefix 2:afa5a01ad84b 307 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
mefix 0:3c99f1705565 308 }
mefix 0:3c99f1705565 309
mefix 0:3c99f1705565 310 int main()
mefix 0:3c99f1705565 311 {
mefix 0:3c99f1705565 312 pc.printf("RESET\n\r");
RiP 4:b83460036800 313
RiP 4:b83460036800 314 // Set the baud rate
mefix 0:3c99f1705565 315 pc.baud(115200);
mefix 0:3c99f1705565 316
RiP 4:b83460036800 317 // Attach the Biquads to the Biquad chains
mefix 0:3c99f1705565 318 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
RiP 4:b83460036800 319 bqc12.add( &bq131);
mefix 0:3c99f1705565 320 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
RiP 4:b83460036800 321 bqc22.add( &bq231);
mefix 0:3c99f1705565 322 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
RiP 4:b83460036800 323 bqc32.add( &bq331);
RiP 4:b83460036800 324
RiP 4:b83460036800 325 // Define the period of the pwm signals
RiP 4:b83460036800 326 motor1.period(0.02f);
RiP 4:b83460036800 327 motor2.period(0.02f);
mefix 0:3c99f1705565 328
RiP 4:b83460036800 329 // Set the direction of the motors to ccw
RiP 5:0581d843fde2 330 motor1dir = 0;
RiP 5:0581d843fde2 331 motor2dir = 0;
RiP 3:58378b78079d 332
RiP 4:b83460036800 333 // Attaching the function my_emg() to the ticker print_timer
RiP 4:b83460036800 334 print_timer.attach(&my_emg, 1);
RiP 3:58378b78079d 335
RiP 4:b83460036800 336 // While loop used for calibrating the emg thresholds, So that every user can use it
RiP 5:0581d843fde2 337 while (servo_button == 1) {
RiP 5:0581d843fde2 338 emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
RiP 5:0581d843fde2 339 emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
RiP 5:0581d843fde2 340 emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
RiP 4:b83460036800 341
RiP 4:b83460036800 342 // Check if the new EMG signal is higher than the current highest value
RiP 5:0581d843fde2 343 if(emg1 > highest_emg1) {
RiP 5:0581d843fde2 344 highest_emg1 = emg1;
RiP 1:ba63033da653 345 }
RiP 4:b83460036800 346
RiP 5:0581d843fde2 347 if(emg2 > highest_emg2) {
RiP 5:0581d843fde2 348 highest_emg2 = emg2;
RiP 1:ba63033da653 349 }
RiP 4:b83460036800 350
RiP 5:0581d843fde2 351 if(emg3 > highest_emg3) {
RiP 5:0581d843fde2 352 highest_emg3 = emg3;
RiP 1:ba63033da653 353 }
RiP 3:58378b78079d 354
RiP 5:0581d843fde2 355 // Define the thresholds as 0.3 times the highest emg value
RiP 5:0581d843fde2 356 threshold1 = 0.30 * highest_emg1;
RiP 5:0581d843fde2 357 threshold2 = 0.30 * highest_emg2;
RiP 5:0581d843fde2 358 threshold3 = 0.30 * highest_emg3;
RiP 1:ba63033da653 359 }
mefix 0:3c99f1705565 360
RiP 5:0581d843fde2 361 // Attach the function activate_controller() to the ticker change_pos_timer
RiP 5:0581d843fde2 362 controller_timer.attach(&activate_controller, Ts);
RiP 4:b83460036800 363
RiP 4:b83460036800 364 // Attach the function my_pos() to the ticker print_timer.
RiP 4:b83460036800 365 print_timer.attach(&my_pos, 1);
mefix 0:3c99f1705565 366
RiP 4:b83460036800 367 // Attach the function activate_servo_control() to the ticker servo_timer
RiP 4:b83460036800 368 servo_timer.attach(&activate_servo_control,servo_Ts);
mefix 0:3c99f1705565 369
mefix 0:3c99f1705565 370 while(1) {
RiP 5:0581d843fde2 371 // Take a sample and control the motor when the go flag is true.
RiP 5:0581d843fde2 372 if (change_controller_go == true) {
RiP 5:0581d843fde2 373 sample();
RiP 5:0581d843fde2 374 motor_controller();
RiP 5:0581d843fde2 375 change_controller_go = false;
mefix 0:3c99f1705565 376 }
RiP 4:b83460036800 377
RiP 5:0581d843fde2 378 // Control the servo when the go flag is true
RiP 5:0581d843fde2 379 if(servo_go == true) {
mefix 0:3c99f1705565 380 servo_controller();
mefix 0:3c99f1705565 381 servo_go=false;
mefix 0:3c99f1705565 382 }
mefix 0:3c99f1705565 383 }
mefix 0:3c99f1705565 384 }