Rick Poppe / Mbed 2 deprecated final_control_v3

naam van een functie veranderd

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@4:b83460036800, 2016-11-05 (annotated)

Committer:
RiP
Date:
Sat Nov 05 16:40:39 2016 +0000
Revision:
4:b83460036800
Parent:
3:58378b78079d
Child:
5:0581d843fde2
overal goed commentaar bij gezet behalve bij de functie PID; ook zijn alle variabelen gegroepeerd

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