Rick Poppe / Mbed 2 deprecated EMG_filter

working version but stripped of most unnecessary code like print statements

Dependencies:   HIDScope MODSERIAL biquadFilter mbed FastPWM QEI

main.cpp@41:a89907bb3f70, 2016-10-27 (annotated)

Committer:
RiP
Date:
Thu Oct 27 14:22:47 2016 +0000
Revision:
41:a89907bb3f70
Parent:
40:1ca50c657cc5
Child:
42:37cd882e7f2b
emg filter en motor aansturing samengevoegd en alles werkt, nu moet alleen het servo gedeelte nog toegevoegd worden

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:32bb76391d89 1 #include "mbed.h"
vsluiter 11:ce72ec658a95 2 #include "HIDScope.h"
RiP 21:3aecd735319d 3 #include "BiQuad.h"
RiP 22:f38a15e851d2 4 #include "MODSERIAL.h"
RiP 41:a89907bb3f70 5 #include "QEI.h"
RiP 41:a89907bb3f70 6 #include "FastPWM.h"
vsluiter 0:32bb76391d89 7
RiP 40:1ca50c657cc5 8 // in gebruik: D(0,1,4,5,6,7,8,10,11,12,13) A(0,1,2)
RiP 40:1ca50c657cc5 9
RiP 39:955929e25858 10 MODSERIAL pc(USBTX, USBRX);
RiP 41:a89907bb3f70 11 HIDScope scope(3); // the amount of scopes to send to the pc
RiP 36:344588e69589 12
vsluiter 4:8b298dfada81 13 //Define objects
RiP 24:01b4b51b5dc6 14
RiP 31:9c3b022f1dc3 15 //Define the EMG inputs
RiP 21:3aecd735319d 16 AnalogIn emg1( A0 );
RiP 21:3aecd735319d 17 AnalogIn emg2( A1 );
RiP 21:3aecd735319d 18 AnalogIn emg3( A2 );
tomlankhorst 19:2bf824669684 19
RiP 41:a89907bb3f70 20 //Define motor outputs
RiP 41:a89907bb3f70 21 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
RiP 41:a89907bb3f70 22 FastPWM motor1(D6); // speed of motor 1
RiP 41:a89907bb3f70 23 FastPWM motor2(D5); //speed of motor 2
RiP 41:a89907bb3f70 24 DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
RiP 41:a89907bb3f70 25
RiP 41:a89907bb3f70 26 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
RiP 41:a89907bb3f70 27 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
RiP 41:a89907bb3f70 28
RiP 31:9c3b022f1dc3 29 //Define the Tickers
RiP 39:955929e25858 30 Ticker pos_timer; // the timer which is used to print the position every second
RiP 39:955929e25858 31 Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
RiP 41:a89907bb3f70 32 Ticker control; // Ticker for processing encoder input
vsluiter 2:e314bb3b2d99 33
RiP 36:344588e69589 34 //Initialize all variables
RiP 39:955929e25858 35 volatile bool sampletimer = false; // a variable which is changed when a sample needs to be taken
RiP 41:a89907bb3f70 36 volatile bool controller_go=false;
RiP 25:1a71424b05ff 37
RiP 39:955929e25858 38 double threshold = 0.04; // the threshold which the emg signals need to surpass to do something
RiP 40:1ca50c657cc5 39 double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
RiP 39:955929e25858 40 double emg02; // the first emg signal
RiP 39:955929e25858 41 double emg12; // the second emg signal
RiP 39:955929e25858 42 double emg22; // the third emg signal
RiP 39:955929e25858 43 double ref_x=0.0000; // the x reference position
RiP 39:955929e25858 44 double ref_y=0.0000; // the y reference position
RiP 39:955929e25858 45 double old_ref_x; // the old x reference
RiP 39:955929e25858 46 double old_ref_y; // the old y reference
RiP 39:955929e25858 47 double speed=0.00002; // the variable with which a speed is reached of 1cm/s
RiP 39:955929e25858 48 double theta=0.0; // angle of the arm
RiP 39:955929e25858 49 double radius=0.0; // radius of the arm
RiP 36:344588e69589 50
RiP 39:955929e25858 51 const double minRadius=0.3; // minimum radius of arm
RiP 39:955929e25858 52 const double maxRadius=0.6; // maximum radius of arm
RiP 39:955929e25858 53 const double minAngle=-1.25; // minimum angle for limiting controller
RiP 40:1ca50c657cc5 54 const double min_y=-0.28; // minimum height which the spatula can reach
RiP 39:955929e25858 55 char key; // variable to place the keyboard input
RiP 28:3b1b29193851 56
RiP 41:a89907bb3f70 57 double m1_pwm=0; //variable for PWM control motor 1
RiP 41:a89907bb3f70 58 double m2_pwm=0; //variable for PWM control motor 2
RiP 41:a89907bb3f70 59
RiP 41:a89907bb3f70 60 const double m1_Kp = 9.974, m1_Ki = 16.49, m1_Kd = 1.341, m1_N = 100; // controller constants motor 1
RiP 41:a89907bb3f70 61 double m1_v1 = 0, m1_v2 = 0; // Memory variables
RiP 41:a89907bb3f70 62 const double m1_Ts = 0.01; // Controller sample time
RiP 41:a89907bb3f70 63
RiP 41:a89907bb3f70 64 const double m2_Kp = 9.974, m2_Ki = 16.49, m2_Kd = 1.341, m2_N = 100; // controller constants motor 2
RiP 41:a89907bb3f70 65 double m2_v1 = 0, m2_v2 = 0; // Memory variables
RiP 41:a89907bb3f70 66 const double m2_Ts = 0.01; // Controller sample time
RiP 41:a89907bb3f70 67
RiP 41:a89907bb3f70 68 const double pi=3.14159265359;
RiP 41:a89907bb3f70 69 const double res = 64/(1/131.25*2*pi); // resolution on gearbox shaft per pulse
RiP 41:a89907bb3f70 70 const double V_max=9.0; // maximum voltage supplied by trafo
RiP 41:a89907bb3f70 71 const double pulleyDiameter=0.0398; // pulley diameter
RiP 41:a89907bb3f70 72
RiP 31:9c3b022f1dc3 73 //Define the needed Biquad chains
RiP 21:3aecd735319d 74 BiQuadChain bqc11;
RiP 21:3aecd735319d 75 BiQuadChain bqc13;
RiP 21:3aecd735319d 76 BiQuadChain bqc21;
RiP 21:3aecd735319d 77 BiQuadChain bqc23;
RiP 21:3aecd735319d 78 BiQuadChain bqc31;
RiP 21:3aecd735319d 79 BiQuadChain bqc33;
RiP 31:9c3b022f1dc3 80
RiP 31:9c3b022f1dc3 81 //Define the BiQuads for the filter of the first emg signal
RiP 31:9c3b022f1dc3 82 //Notch filter
RiP 21:3aecd735319d 83 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
RiP 21:3aecd735319d 84 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
RiP 21:3aecd735319d 85 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
RiP 31:9c3b022f1dc3 86 //High pass filter
RiP 36:344588e69589 87 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
RiP 36:344588e69589 88 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
RiP 36:344588e69589 89 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
RiP 36:344588e69589 90 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
RiP 31:9c3b022f1dc3 91 //Low pass filter
RiP 21:3aecd735319d 92 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
RiP 21:3aecd735319d 93
RiP 31:9c3b022f1dc3 94 //Define the BiQuads for the filter of the second emg signal
RiP 31:9c3b022f1dc3 95 //Notch filter
RiP 31:9c3b022f1dc3 96 BiQuad bq211 = bq111;
RiP 31:9c3b022f1dc3 97 BiQuad bq212 = bq112;
RiP 31:9c3b022f1dc3 98 BiQuad bq213 = bq113;
RiP 39:955929e25858 99 //High pass filter
RiP 31:9c3b022f1dc3 100 BiQuad bq221 = bq121;
RiP 36:344588e69589 101 BiQuad bq222 = bq122;
RiP 36:344588e69589 102 BiQuad bq223 = bq123;
RiP 39:955929e25858 103 //Low pass filter
RiP 31:9c3b022f1dc3 104 BiQuad bq231 = bq131;
RiP 21:3aecd735319d 105
RiP 31:9c3b022f1dc3 106 //Define the BiQuads for the filter of the third emg signal
RiP 31:9c3b022f1dc3 107 //notch filter
RiP 31:9c3b022f1dc3 108 BiQuad bq311 = bq111;
RiP 31:9c3b022f1dc3 109 BiQuad bq312 = bq112;
RiP 31:9c3b022f1dc3 110 BiQuad bq313 = bq113;
RiP 31:9c3b022f1dc3 111 //High pass filter
RiP 31:9c3b022f1dc3 112 BiQuad bq321 = bq121;
RiP 36:344588e69589 113 BiQuad bq323 = bq122;
RiP 36:344588e69589 114 BiQuad bq322 = bq123;
RiP 31:9c3b022f1dc3 115 //low pass filter
RiP 31:9c3b022f1dc3 116 BiQuad bq331 = bq131;
RiP 22:f38a15e851d2 117
RiP 22:f38a15e851d2 118 void sampleflag()
RiP 22:f38a15e851d2 119 {
mefix 38:0c8a6b0834da 120 if (sampletimer==true) {
RiP 39:955929e25858 121 // this if statement is used to see if the code takes too long before it is called again
RiP 39:955929e25858 122 pc.printf("rate too high error in sampleflag\n\r");
RiP 37:af85a7b57a25 123 }
RiP 36:344588e69589 124 //This sets the go flag for when the function sample needs to be called
RiP 22:f38a15e851d2 125 sampletimer=true;
RiP 22:f38a15e851d2 126 }
RiP 22:f38a15e851d2 127
RiP 41:a89907bb3f70 128 void activate_controller()
RiP 24:01b4b51b5dc6 129 {
RiP 41:a89907bb3f70 130 if (controller_go==true) {
RiP 41:a89907bb3f70 131 // this if statement is used to see if the code takes too long before it is called again
RiP 41:a89907bb3f70 132 pc.printf("rate too high error in activate_controller()\n\r");
RiP 41:a89907bb3f70 133 }
RiP 41:a89907bb3f70 134 controller_go=true; //activate go flag
RiP 24:01b4b51b5dc6 135 }
RiP 22:f38a15e851d2 136
RiP 39:955929e25858 137 void sample()
vsluiter 2:e314bb3b2d99 138 {
RiP 39:955929e25858 139 //This checks if a key is pressed and changes the variable key in the pressed key
RiP 33:fcd4568f1c86 140 if (pc.readable()==1) {
RiP 33:fcd4568f1c86 141 key=pc.getc();
mefix 38:0c8a6b0834da 142 }
RiP 36:344588e69589 143 //Read the emg signals and filter it
RiP 22:f38a15e851d2 144
RiP 36:344588e69589 145 emg02=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 0
RiP 36:344588e69589 146 emg12=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 1
RiP 36:344588e69589 147 emg22=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 2
RiP 29:ac08c1a32c54 148
RiP 39:955929e25858 149 //remember what the reference was
RiP 37:af85a7b57a25 150 old_ref_x=ref_x;
RiP 37:af85a7b57a25 151 old_ref_y=ref_y;
RiP 39:955929e25858 152 //look if the emg signals go over the threshold and change the reference accordingly
RiP 33:fcd4568f1c86 153 if (emg02>threshold&&emg12>threshold&&emg22>threshold || key=='d') {
RiP 37:af85a7b57a25 154 ref_x=ref_x-speed;
RiP 37:af85a7b57a25 155 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 156
RiP 33:fcd4568f1c86 157 } else if (emg02>threshold&&emg12>threshold || key == 'a' ) {
RiP 37:af85a7b57a25 158 ref_x=ref_x-speed;
RiP 23:54d28f9eef53 159
RiP 33:fcd4568f1c86 160 } else if (emg02>threshold&&emg22>threshold || key == 's') {
RiP 37:af85a7b57a25 161 ref_y=ref_y-speed;
RiP 31:9c3b022f1dc3 162
RiP 33:fcd4568f1c86 163 } else if (emg12>threshold&&emg22>threshold || key == 'e' ) {
RiP 31:9c3b022f1dc3 164 ref_x=ref_x+speed;
RiP 31:9c3b022f1dc3 165 ref_y=ref_y+speed;
RiP 31:9c3b022f1dc3 166
RiP 33:fcd4568f1c86 167 } else if (emg12>threshold || key == 'q' ) {
RiP 31:9c3b022f1dc3 168 ref_x=ref_x+speed;
RiP 40:1ca50c657cc5 169
RiP 33:fcd4568f1c86 170 } else if (emg22>threshold || key == 'w') {
RiP 31:9c3b022f1dc3 171 ref_y=ref_y+speed;
RiP 37:af85a7b57a25 172 }
RiP 37:af85a7b57a25 173
RiP 39:955929e25858 174 // convert the x and y reference to the theta and radius reference
mefix 38:0c8a6b0834da 175 theta=atan(ref_y/(ref_x+minRadius));
mefix 38:0c8a6b0834da 176 radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
mefix 38:0c8a6b0834da 177
RiP 39:955929e25858 178 //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
mefix 38:0c8a6b0834da 179 if (theta < minAngle) {
RiP 37:af85a7b57a25 180 ref_x=old_ref_x;
RiP 37:af85a7b57a25 181 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 182
RiP 37:af85a7b57a25 183 } else if (radius < minRadius) {
RiP 37:af85a7b57a25 184 ref_x=old_ref_x;
RiP 37:af85a7b57a25 185 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 186
mefix 38:0c8a6b0834da 187 } else if ( radius > maxRadius) {
RiP 37:af85a7b57a25 188 ref_x=old_ref_x;
RiP 37:af85a7b57a25 189 ref_y=old_ref_y;
RiP 40:1ca50c657cc5 190 } else if (ref_y<min_y) {
RiP 40:1ca50c657cc5 191 ref_x=old_ref_x;
RiP 40:1ca50c657cc5 192 ref_y=old_ref_y;
RiP 22:f38a15e851d2 193 }
RiP 41:a89907bb3f70 194 theta=atan(ref_y/(ref_x+minRadius));
RiP 41:a89907bb3f70 195 radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
RiP 41:a89907bb3f70 196 }
RiP 41:a89907bb3f70 197
RiP 41:a89907bb3f70 198 double PID( double err, const double Kp, const double Ki, const double Kd,
RiP 41:a89907bb3f70 199 const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
RiP 41:a89907bb3f70 200 {
RiP 41:a89907bb3f70 201 const double a1 =-4/(N*Ts+2),
RiP 41:a89907bb3f70 202 a2=-(N*Ts-2)/(N*Ts+2),
RiP 41:a89907bb3f70 203 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
RiP 41:a89907bb3f70 204 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
RiP 41:a89907bb3f70 205 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
RiP 41:a89907bb3f70 206
RiP 41:a89907bb3f70 207 double v=err-a1*v1-a2*v2;
RiP 41:a89907bb3f70 208 double u=b0*v+b1*v1+b2*v2;
RiP 41:a89907bb3f70 209 v2=v1;
RiP 41:a89907bb3f70 210 v1=v;
RiP 41:a89907bb3f70 211 return u;
RiP 41:a89907bb3f70 212 }
RiP 41:a89907bb3f70 213
RiP 41:a89907bb3f70 214 void controller() //function for executing controller action
RiP 41:a89907bb3f70 215 {
RiP 41:a89907bb3f70 216
RiP 41:a89907bb3f70 217 //converting radius and theta to gearbox angle
RiP 41:a89907bb3f70 218 double ref_angle1=16*theta;
RiP 41:a89907bb3f70 219 double ref_angle2=(-radius+minRadius)/pi/pulleyDiameter;
RiP 41:a89907bb3f70 220 //pc.printf("%.5f",ref_angle2);
RiP 41:a89907bb3f70 221 //ref_angle2=0.0;
RiP 41:a89907bb3f70 222
RiP 41:a89907bb3f70 223 double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
RiP 41:a89907bb3f70 224 double angle2 = Encoder2.getPulses()/res; //get number of pulses
RiP 41:a89907bb3f70 225 m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
RiP 41:a89907bb3f70 226 //divide by voltage to get pwm duty cycle percentage)
RiP 41:a89907bb3f70 227 m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
RiP 41:a89907bb3f70 228
RiP 41:a89907bb3f70 229 //limit pwm value and change motor direction when pwm becomes either negative or positive
RiP 41:a89907bb3f70 230 if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
RiP 41:a89907bb3f70 231 motor1dir=0;
RiP 41:a89907bb3f70 232 motor1.write(m1_pwm);
RiP 41:a89907bb3f70 233 } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
RiP 41:a89907bb3f70 234 motor1dir=1;
RiP 41:a89907bb3f70 235 motor1.write(-m1_pwm);
RiP 41:a89907bb3f70 236 }
RiP 41:a89907bb3f70 237
RiP 41:a89907bb3f70 238 if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
RiP 41:a89907bb3f70 239 motor2dir=0;
RiP 41:a89907bb3f70 240 motor2.write(m2_pwm);
RiP 41:a89907bb3f70 241 } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
RiP 41:a89907bb3f70 242 motor2dir=1;
RiP 41:a89907bb3f70 243 motor2.write(-m2_pwm);
RiP 41:a89907bb3f70 244 }
RiP 41:a89907bb3f70 245
RiP 41:a89907bb3f70 246 //hidsopce to check what the code does exactly
RiP 41:a89907bb3f70 247 scope.set(0,ref_angle2-angle2); //error
RiP 41:a89907bb3f70 248 scope.set(1,angle2);
RiP 41:a89907bb3f70 249 scope.set(2,m2_pwm);
RiP 41:a89907bb3f70 250 scope.send();
vsluiter 2:e314bb3b2d99 251 }
vsluiter 0:32bb76391d89 252
RiP 27:1ff7fa636f1c 253 void my_pos()
RiP 27:1ff7fa636f1c 254 {
RiP 36:344588e69589 255 //This function is attached to a ticker so that the reference position is printed every second.
RiP 37:af85a7b57a25 256 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta);
RiP 27:1ff7fa636f1c 257
RiP 27:1ff7fa636f1c 258 }
RiP 26:91d48c0b722d 259
vsluiter 0:32bb76391d89 260 int main()
RiP 21:3aecd735319d 261 {
RiP 29:ac08c1a32c54 262 pc.printf("RESET\n\r");
RiP 22:f38a15e851d2 263 pc.baud(115200);
RiP 22:f38a15e851d2 264
RiP 39:955929e25858 265 //Attach the Biquads to the Biquad chains
RiP 36:344588e69589 266 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
RiP 21:3aecd735319d 267 bqc13.add( &bq131);
RiP 36:344588e69589 268 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
RiP 21:3aecd735319d 269 bqc23.add( &bq231);
RiP 36:344588e69589 270 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
RiP 21:3aecd735319d 271 bqc33.add( &bq331);
RiP 36:344588e69589 272
RiP 41:a89907bb3f70 273 motor1.period(0.02f); //period of pwm signal for motor 1
RiP 41:a89907bb3f70 274 motor2.period(0.02f); // period of pwm signal for motor 2
RiP 41:a89907bb3f70 275 motor1dir=0; // setting direction to ccw
RiP 41:a89907bb3f70 276 motor2dir=0; // setting direction to ccw
RiP 41:a89907bb3f70 277
RiP 36:344588e69589 278 //Attach the 'sample' function to the timer 'sample_timer'.
RiP 36:344588e69589 279 //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
RiP 23:54d28f9eef53 280 sample_timer.attach(&sampleflag, samplefreq);
RiP 41:a89907bb3f70 281
RiP 36:344588e69589 282 //Attach the function my_pos to the timer pos_timer.
RiP 36:344588e69589 283 //This ensures that the position is printed every second.
RiP 31:9c3b022f1dc3 284 pos_timer.attach(&my_pos, 1);
RiP 41:a89907bb3f70 285 control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
RiP 41:a89907bb3f70 286
tomlankhorst 15:0da764eea774 287
RiP 22:f38a15e851d2 288 while(1) {
RiP 39:955929e25858 289 //Only take a sample when the go flag is true.
RiP 22:f38a15e851d2 290 if (sampletimer==true) {
RiP 39:955929e25858 291 sample();
RiP 39:955929e25858 292 sampletimer = false; //change sampletimer to false if sample() is finished
RiP 25:1a71424b05ff 293 }
RiP 41:a89907bb3f70 294 if(controller_go) { // go flag
RiP 41:a89907bb3f70 295 controller();
RiP 41:a89907bb3f70 296 controller_go=false;
RiP 41:a89907bb3f70 297 }
RiP 22:f38a15e851d2 298 }
vsluiter 0:32bb76391d89 299 }