Rick Poppe / Mbed 2 deprecated final_control_v4

code die in het verslag komt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@9:ef6c5feb5623, 2016-11-07 (annotated)

Committer:
RiP
Date:
Mon Nov 07 13:17:21 2016 +0000
Revision:
9:ef6c5feb5623
Parent:
8:9dcd1603d713 
totale code

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