Rick Poppe / Mbed 2 deprecated final_control_v4

code die in het verslag komt

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@8:9dcd1603d713, 2016-11-06 (annotated)

Committer:
RiP
Date:
Sun Nov 06 13:28:32 2016 +0000
Revision:
8:9dcd1603d713
Parent:
7:bb51e2421953
Child:
9:ef6c5feb5623
de code die in het verslag komt

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),
mefix 6:2225bdcfcd6e 9 // 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)
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 8:9dcd1603d713 14 AnalogIn emg_in1( A0 ); // EMG signal of the thumb
RiP 8:9dcd1603d713 15 AnalogIn emg_in2( A1 ); // EMG signal of the right bicep
RiP 8:9dcd1603d713 16 AnalogIn emg_in3( A2 ); // EMG signal of the left bicep
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
mefix 6:2225bdcfcd6e 21 FastPWM motor1(D6); // Duty cycle of pwm signal for motor 1
mefix 6:2225bdcfcd6e 22 FastPWM motor2(D5); // Duty cycle of pwm signal for motor 2
mefix 6:2225bdcfcd6e 23 FastPWM servo(D9); // Pulsewidth of pwm signal for servo
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
mefix 6:2225bdcfcd6e 29 QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //Defining highest encoder accuracy for motor1
mefix 6:2225bdcfcd6e 30 QEI encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //Defining highest encoder accuracy for motor2
mefix 0:3c99f1705565 31
RiP 4:b83460036800 32 // Define the Tickers
mefix 6:2225bdcfcd6e 33 Ticker print_timer; // Ticker for printing position or highest EMG values
mefix 6:2225bdcfcd6e 34 Ticker controller_timer; // Ticker for sampling and motor control
mefix 6:2225bdcfcd6e 35 Ticker servo_timer; // Ticker for servo control
mefix 0:3c99f1705565 36
RiP 4:b83460036800 37 // Define the Time constants
mefix 6:2225bdcfcd6e 38 const double Ts = 0.002; // Time constant for sampling and motor control
mefix 6:2225bdcfcd6e 39 const double servo_Ts = 0.02; // Time constant for servo control
mefix 0:3c99f1705565 40
RiP 4:b83460036800 41 // Define the go flags
RiP 8:9dcd1603d713 42 volatile bool 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
mefix 6:2225bdcfcd6e 48 double threshold1, threshold2, threshold3; // The threshold for the EMG signals to change the reference
RiP 1:ba63033da653 49
RiP 4:b83460036800 50 //Define the keyboard input
mefix 6:2225bdcfcd6e 51 char key; // Stores last pressed key
mefix 0:3c99f1705565 52
RiP 4:b83460036800 53 // Define the reference variables
mefix 6:2225bdcfcd6e 54 double ref_x = 0.0, ref_y = 0.0; // Reference position
mefix 6:2225bdcfcd6e 55 double old_ref_x, old_ref_y; // Old reference position
mefix 6:2225bdcfcd6e 56 double speed = 0.00006; // Variable with which a speed is reached of 3 cm/s in x and y direction
mefix 6:2225bdcfcd6e 57 double theta = 0.0; // Angle reference of the arm
mefix 6:2225bdcfcd6e 58 double radius = 0.0; // 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
mefix 6:2225bdcfcd6e 64 const double min_y = -0.26; // The minimum y position of the arm
mefix 0:3c99f1705565 65
RiP 4:b83460036800 66 // Define variables of motor 1
mefix 6:2225bdcfcd6e 67 double m1_pwm = 0; // Variable for PWM motor 1
mefix 6:2225bdcfcd6e 68 const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // PID values for 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
mefix 6:2225bdcfcd6e 72 double m2_pwm = 0; // Variable for PWM motor 2
mefix 6:2225bdcfcd6e 73 const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // PID values for 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
mefix 6:2225bdcfcd6e 83 double servo_pwm = 0.0023; // Pulsewidth PWM for servo (min 0.0005, max 0.0025)
mefix 6:2225bdcfcd6e 84 const double min_theta = -37.0 / 180.0 * pi; // Minimum angle of the arm
mefix 6:2225bdcfcd6e 85 const double max_theta = -14.0 / 180.0 * pi; // Maximum angle to which the spatula is stabilised
mefix 6:2225bdcfcd6e 86 const double diff_theta = max_theta - min_theta; // Difference between max and min angle
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
mefix 6:2225bdcfcd6e 135 void activate_controller() // Go flag for sample() and controller()
mefix 0:3c99f1705565 136 {
mefix 6:2225bdcfcd6e 137 if (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 6:2225bdcfcd6e 140
mefix 0:3c99f1705565 141 }
mefix 6:2225bdcfcd6e 142 controller_go = true; // Activate go flag
mefix 0:3c99f1705565 143 }
mefix 0:3c99f1705565 144
mefix 6:2225bdcfcd6e 145 void activate_servo_control() // Go flag for servo_controller()
mefix 0:3c99f1705565 146 {
RiP 5:0581d843fde2 147 if (servo_go == true) {
RiP 4:b83460036800 148 // This if statement is used to see if the code takes too long before it is called again
mefix 6:2225bdcfcd6e 149 pc.printf("rate too high, error in activate_servo_control()\n\r");
mefix 0:3c99f1705565 150 }
RiP 5:0581d843fde2 151 servo_go = true; // Activate go flag
mefix 0:3c99f1705565 152 }
mefix 0:3c99f1705565 153
mefix 6:2225bdcfcd6e 154 void sample() // Function for sampling emg signal and changing reference position
mefix 0:3c99f1705565 155 {
RiP 4:b83460036800 156 // Change key if the keyboard is pressed
RiP 5:0581d843fde2 157 if (pc.readable() == 1) {
mefix 0:3c99f1705565 158 key=pc.getc();
mefix 0:3c99f1705565 159 }
RiP 4:b83460036800 160
RiP 4:b83460036800 161 // Read the emg signals and filter it
RiP 5:0581d843fde2 162 emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
RiP 5:0581d843fde2 163 emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
RiP 5:0581d843fde2 164 emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
mefix 0:3c99f1705565 165
RiP 4:b83460036800 166 // Remember what the old reference was
RiP 5:0581d843fde2 167 old_ref_x = ref_x;
RiP 5:0581d843fde2 168 old_ref_y = ref_y;
RiP 4:b83460036800 169
mefix 6:2225bdcfcd6e 170 // Change the reference position if emg signals exceed threshold value or if key is pressed
RiP 5:0581d843fde2 171 if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd') // Negative XY direction
RiP 4:b83460036800 172 {
RiP 5:0581d843fde2 173 ref_x = ref_x - speed;
RiP 5:0581d843fde2 174 ref_y = ref_y - speed;
mefix 0:3c99f1705565 175
RiP 5:0581d843fde2 176 } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z') // Negative X direction
RiP 4:b83460036800 177 {
RiP 5:0581d843fde2 178 ref_x = ref_x - speed;
RiP 4:b83460036800 179
RiP 5:0581d843fde2 180 } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's') // Negative Y direction
RiP 4:b83460036800 181 {
RiP 5:0581d843fde2 182 ref_y = ref_y - speed;
mefix 0:3c99f1705565 183
RiP 5:0581d843fde2 184 } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' ) // Positive XY direction
RiP 4:b83460036800 185 {
RiP 5:0581d843fde2 186 ref_x = ref_x + speed;
RiP 5:0581d843fde2 187 ref_y = ref_y + speed;
mefix 0:3c99f1705565 188
RiP 5:0581d843fde2 189 } else if (emg2 > threshold2 || key == 'q' ) // Positive X direction
RiP 4:b83460036800 190 {
RiP 5:0581d843fde2 191 ref_x = ref_x + speed;
mefix 0:3c99f1705565 192
RiP 5:0581d843fde2 193 } else if (emg3 > threshold3 || key == 'w') // Positive Y direction
RiP 4:b83460036800 194 {
RiP 5:0581d843fde2 195 ref_y = ref_y + speed;
mefix 0:3c99f1705565 196 }
mefix 0:3c99f1705565 197
RiP 4:b83460036800 198 // Change z_pushed to true if 'z' is pressed on the keyboard
RiP 3:58378b78079d 199 if (key == 'z') {
RiP 5:0581d843fde2 200 z_pushed = true;
RiP 3:58378b78079d 201 }
RiP 3:58378b78079d 202
RiP 4:b83460036800 203 // Reset the reference and the encoders if z is no longer pressed
RiP 4:b83460036800 204 if (key != 'z' && z_pushed) {
RiP 5:0581d843fde2 205 ref_x = 0.0;
RiP 5:0581d843fde2 206 ref_y = 0.0;
RiP 5:0581d843fde2 207 encoder1.reset();
RiP 5:0581d843fde2 208 encoder2.reset();
RiP 5:0581d843fde2 209 z_pushed = false;
mefix 0:3c99f1705565 210 }
mefix 0:3c99f1705565 211
RiP 4:b83460036800 212 // Convert the x and y reference to the theta and radius reference
RiP 5:0581d843fde2 213 theta = atan( ref_y / (ref_x + min_radius));
RiP 5:0581d843fde2 214 radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
mefix 0:3c99f1705565 215
RiP 4:b83460036800 216 // If the new reference is outside the possible range then revert back to the old reference unless z is pressed
RiP 4:b83460036800 217 if (radius < min_radius) {
mefix 0:3c99f1705565 218 if (key != 'z') {
RiP 5:0581d843fde2 219 ref_x = old_ref_x;
RiP 5:0581d843fde2 220 ref_y = old_ref_y;
mefix 0:3c99f1705565 221 }
RiP 4:b83460036800 222 } else if ( radius > max_radius) {
RiP 5:0581d843fde2 223 ref_x = old_ref_x;
RiP 5:0581d843fde2 224 ref_y = old_ref_y;
RiP 5:0581d843fde2 225 } else if (ref_y < min_y) {
RiP 5:0581d843fde2 226 ref_y = old_ref_y;
mefix 0:3c99f1705565 227 }
RiP 4:b83460036800 228
RiP 4:b83460036800 229 // Calculate theta and radius again
RiP 5:0581d843fde2 230 theta = atan( ref_y / (ref_x + min_radius));
RiP 5:0581d843fde2 231 radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
mefix 0:3c99f1705565 232 }
mefix 0:3c99f1705565 233
mefix 0:3c99f1705565 234 double PID( double err, const double Kp, const double Ki, const double Kd,
mefix 6:2225bdcfcd6e 235 const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF controller (tustin approximation)
mefix 0:3c99f1705565 236 {
RiP 5:0581d843fde2 237 const double a1 = -4 / (N * Ts + 2),
RiP 5:0581d843fde2 238 a2 = -(N * Ts - 2)/(N*Ts + 2),
RiP 5:0581d843fde2 239 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 240 b1 = (Ki * N * pow(Ts, 2) - 4 * Kp - 4 * Kd * N) / (N * Ts + 2),
mefix 6:2225bdcfcd6e 241 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
mefix 0:3c99f1705565 242
RiP 5:0581d843fde2 243 double v = err - a1 * v1 - a2 * v2;
mefix 6:2225bdcfcd6e 244 double u = b0 * v + b1 * v1 + b2 * v2; //input for motors
mefix 6:2225bdcfcd6e 245 v2 = v1; //store old value
mefix 6:2225bdcfcd6e 246 v1 = v; //store old value
mefix 0:3c99f1705565 247 return u;
mefix 0:3c99f1705565 248 }
mefix 0:3c99f1705565 249
RiP 5:0581d843fde2 250 void motor_controller() // Function for executing controller action
mefix 0:3c99f1705565 251 {
RiP 4:b83460036800 252 // Convert radius and theta to gearbox angles
RiP 5:0581d843fde2 253 double ref_angle1 = 16 * theta;
RiP 5:0581d843fde2 254 double ref_angle2 = (-radius + min_radius) / pulley_radius;
mefix 0:3c99f1705565 255
RiP 4:b83460036800 256 // Get number of pulses of the encoders
RiP 5:0581d843fde2 257 double angle1 = encoder1.getPulses() / res; //counterclockwise is positive
RiP 5:0581d843fde2 258 double angle2 = encoder2.getPulses() / res;
RiP 4:b83460036800 259
RiP 4:b83460036800 260 // Calculate the motor pwm using the function PID() and the voltage
RiP 5:0581d843fde2 261 m1_pwm = (PID(ref_angle1 - angle1, m1_Kp, m1_Ki, m1_Kd, Ts, m1_N, m1_v1, m1_v2)) / V_max;
RiP 5:0581d843fde2 262 m2_pwm = (PID(ref_angle2 - angle2, m2_Kp, m2_Ki, m2_Kd, Ts, m2_N, m2_v1, m2_v2)) / V_max;
mefix 0:3c99f1705565 263
RiP 4:b83460036800 264 // Limit pwm value and change motor direction when pwm becomes either negative or positive
RiP 5:0581d843fde2 265 if (m1_pwm >= 0.0f && m1_pwm <= 1.0f) {
RiP 5:0581d843fde2 266 motor1dir = 0;
mefix 0:3c99f1705565 267 motor1.write(m1_pwm);
mefix 0:3c99f1705565 268 } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
RiP 5:0581d843fde2 269 motor1dir = 1;
mefix 0:3c99f1705565 270 motor1.write(-m1_pwm);
mefix 0:3c99f1705565 271 }
mefix 0:3c99f1705565 272
RiP 5:0581d843fde2 273 if (m2_pwm >= 0.0f && m2_pwm <= 1.0f) {
RiP 5:0581d843fde2 274 motor2dir = 0;
mefix 0:3c99f1705565 275 motor2.write(m2_pwm);
mefix 0:3c99f1705565 276 } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
RiP 5:0581d843fde2 277 motor2dir = 1;
mefix 0:3c99f1705565 278 motor2.write(-m2_pwm);
mefix 0:3c99f1705565 279 }
mefix 0:3c99f1705565 280 }
mefix 0:3c99f1705565 281
mefix 6:2225bdcfcd6e 282 void servo_controller() // Function for stabilizing the spatula with the servo
mefix 0:3c99f1705565 283 {
mefix 6:2225bdcfcd6e 284 // If theta is smaller than max_theta, servo_pwm is adjusted to stabilise spatula
mefix 6:2225bdcfcd6e 285 if (theta < max_theta ) {
RiP 5:0581d843fde2 286 servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
mefix 0:3c99f1705565 287 } else {
RiP 5:0581d843fde2 288 servo_pwm = max_servo_pwm;
mefix 0:3c99f1705565 289 }
RiP 4:b83460036800 290
mefix 6:2225bdcfcd6e 291 // Spatula goes to its maximum position to flip a burger if button is pressed
RiP 4:b83460036800 292 if (!servo_button) {
RiP 5:0581d843fde2 293 servo_pwm = 0.0014;
RiP 3:58378b78079d 294 }
RiP 3:58378b78079d 295
RiP 4:b83460036800 296 // Send the servo_pwm to the servo
mefix 0:3c99f1705565 297 servo.pulsewidth(servo_pwm);
mefix 0:3c99f1705565 298 }
mefix 0:3c99f1705565 299
mefix 6:2225bdcfcd6e 300 void my_emg() // This function prints the highest emg values
RiP 1:ba63033da653 301 {
RiP 1:ba63033da653 302 pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
RiP 1:ba63033da653 303 }
RiP 1:ba63033da653 304
mefix 0:3c99f1705565 305
mefix 6:2225bdcfcd6e 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
mefix 6:2225bdcfcd6e 337 // While loop used for calibrating 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 5:0581d843fde2 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 }