Rick Poppe / Mbed 2 deprecated Biorobotics

merged EMG and PID codes

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@3:511a14a12629, 2016-10-27 (annotated)

Committer:
RiP
Date:
Thu Oct 27 08:43:37 2016 +0000
Revision:
3:511a14a12629
Parent:
2:625837aa7a56 
merged EMG and Motor Control(PID)

Who changed what in which revision?

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