Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "QEI.h" 00003 #include "math.h" 00004 00005 00006 // Motor 1 & 2 00007 DigitalOut Direction(D4); 00008 PwmOut PowerMotor(D5); 00009 QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); 00010 DigitalOut Direction2(D7); 00011 PwmOut PowerMotor2(D6); 00012 QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); 00013 PwmOut PowerServo(D3); 00014 00015 // Buttons & EMG 00016 AnalogIn Button(A3); // Calibration button 00017 AnalogIn Button2(A4); // Fire button 00018 DigitalIn Button3(D9); // Home button 00019 AnalogIn EMG(A0); 00020 AnalogIn EMG2(A1); 00021 00022 // Tickers & timers 00023 00024 Ticker biquadTicker; 00025 Ticker MotorWrite; 00026 Timer timer; 00027 Timer TijdEMGCal; 00028 00029 // LED 00030 DigitalOut ledF(LED_RED); 00031 DigitalOut ledC(LED_GREEN); 00032 00033 00034 00035 // Values 00036 const double twopi = 6.2831853071795; 00037 const double Fs=100; // Sample Frequency 00038 int Fired = 0; 00039 00040 // Motor 1 (Translation) 00041 const double n1 = 3.3; // The amount of the rotations that is allowed 00042 int Pulses; 00043 double Rotatie = 0; //Rotation in radial 00044 double Goal; // Setpoint for Motor 00045 double Error = 0; // Setting initial values 00046 double Errord = 0; 00047 double Errori = 0; 00048 double Errorp = 0; 00049 const double Kp = 0.2; // PID controll parameters 00050 const double Kd = 10; 00051 const double Ki = 0.2; 00052 double v = 0; 00053 double upperlimit; //Maximum rounds up 00054 const double downlimit = 0.8; //Lowest limit of the robot 00055 const double margin = 0.8; //Margin for safe range 00056 bool OutRange = false; //In Range or not 00057 00058 // Motor 2 (Rotation) 00059 const double n2 = 0.334; 00060 int Pulses2; 00061 double Rotatie2 = 0; 00062 double Goal2; 00063 double Error2 = 0; 00064 double Errord2 = 0; 00065 double Errori2 = 0; 00066 double Errorp2 = 0; 00067 const double Kp2 = 0.2; 00068 const double Kd2 = 10; 00069 const double Ki2 = 0.2; 00070 double v2 = 0; 00071 double turnlimit = 0.4; 00072 // Margin 2 = 0 00073 bool OutRange2 = false; 00074 00075 // Switch between firing mode (Excecute) and return to Home mode 00076 bool Excecute = false; 00077 bool Home = false; 00078 00079 // Filter 00080 double Fs2 = 500; // Frequency in Hz 00081 const double TijdCal = 5; // Time for calibration 00082 double Max = 0; // Max value for EMG 00083 double Max2 = 0; // Max value for EMG2 00084 bool Cali = false; // Switch for Calibration 00085 double T1 = 0; // Thresholds for EMG 00086 double T2 = 0; 00087 double T3 = 0; 00088 double T4 = 0; 00089 00090 double u1; //All variables for filtering 00091 double y1; 00092 double y2; 00093 double y3; 00094 double y4; 00095 double y5; 00096 double y6; 00097 double y7; 00098 double y8; 00099 double y9; 00100 double y10; 00101 00102 double u10; 00103 double y11; 00104 double y12; 00105 double y13; 00106 double y14; 00107 double y15; 00108 double y16; 00109 double y17; 00110 double y18; 00111 00112 double f1_v1=0,f1_v2=0; 00113 double f2_v1=0,f2_v2=0; 00114 double f3_v1=0,f3_v2=0; 00115 double f4_v1=0,f4_v2=0; 00116 double f5_v1=0,f5_v2=0; 00117 double f6_v1=0,f6_v2=0; 00118 double f7_v1=0,f7_v2=0; 00119 00120 double f1_v3=0,f1_v4=0; 00121 double f2_v3=0,f2_v4=0; 00122 double f3_v3=0,f3_v4=0; 00123 double f4_v3=0,f4_v4=0; 00124 double f5_v3=0,f5_v4=0; 00125 double f6_v3=0,f6_v4=0; 00126 double f7_v3=0,f7_v4=0; 00127 00128 // Notch 00129 const double gainNotch3=0.969922; 00130 const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000; 00131 const double gainNotch4=0.975183; 00132 const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000; 00133 const double gainNotch5=0.993678; 00134 const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000; 00135 00136 // High pass 00137 const double gainHP1=0.96860379641660377; 00138 const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.000000000; 00139 const double gainHP2=0.935820; 00140 const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000; 00141 00142 // Low pass 00143 const double gainLP6=0.000048; 00144 const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000; 00145 const double gainLP7=0.000048; 00146 const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000; 00147 00148 00149 00150 // Voids for calculations in the main programme 00151 00152 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) 00153 { 00154 double v = u-a1*v1-a2*v2; 00155 double y=b0*v+b1*v1+b2*v2; 00156 v2=v1; 00157 v1=v; 00158 return y; 00159 } 00160 00161 void myController() 00162 { 00163 // EMG 1 00164 u1 = EMG.read(); 00165 // Notch 00166 y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); 00167 y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); 00168 y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); 00169 00170 // HP 00171 y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); 00172 y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); 00173 00174 // LP 00175 y6 = fabs(y5); 00176 y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); 00177 y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); 00178 00179 00180 // EMG 2 00181 u10 = EMG2.read(); 00182 // Notch 00183 y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); 00184 y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); 00185 y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); 00186 00187 // HP 00188 y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); 00189 y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); 00190 00191 // LP 00192 y16 = fabs(y15); 00193 y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); 00194 y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); 00195 00196 if (Cali == true) { 00197 if (y8 >= Max) { 00198 Max = y8; 00199 } 00200 if (y18 >= Max2) { 00201 Max2 = y18; 00202 } 00203 } 00204 } 00205 00206 void MotorSet() 00207 { 00208 // Calculation for position of motor 00209 Pulses = Encoder.getPulses(); 00210 Rotatie = (Pulses*twopi)/4200; 00211 Pulses2 = Encoder2.getPulses(); 00212 Rotatie2 = (Pulses2*twopi)/4200; 00213 // Motor 1 (translation) 00214 if (OutRange) { //If the Robot is OutRange or is going Home 00215 Error = Goal-Rotatie; 00216 Errord = (Error-Errorp)/Fs; 00217 Errorp = Error; 00218 if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small 00219 Errori = Errori + Error*1/Fs; 00220 } else { 00221 Errori = 0; 00222 } 00223 if (Error>=0) { // Determine the direction 00224 Direction=1; 00225 } else { 00226 Direction=0; 00227 } 00228 v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed 00229 if (Home == true) { //Maximum speed when OutRange 00230 if (v >0.15) { 00231 v = 0.15; 00232 } 00233 } 00234 } 00235 PowerMotor.write(fabs(v)); 00236 00237 // Dan motor 2 (rotatie) 00238 if (OutRange2) { 00239 Error2 = Goal2-Rotatie2; 00240 Errord2 = (Error2-Errorp2)/Fs; 00241 Errorp2 = Error2; 00242 if (fabs(Error2) <= 0.5) { 00243 Errori2 = Errori2 + Error2*1/Fs; 00244 } else { 00245 Errori2 = 0; 00246 } 00247 if (Error2>=0) { 00248 Direction2 = 0; 00249 } else { 00250 Direction2 = 1; 00251 } 00252 v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; 00253 } 00254 PowerMotor2.write(fabs(v2)); 00255 } 00256 00257 void Calibration () 00258 { 00259 if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range 00260 if (Button >= 0.8) { 00261 wait (0.2); 00262 Cali = true; // Calibration part in the EMG void is being activated 00263 TijdEMGCal.start(); 00264 Excecute = false; 00265 ledC = 0; 00266 v = 0; 00267 v2 = 0; 00268 } 00269 } 00270 if (TijdEMGCal.read() >= TijdCal) { 00271 Cali = false; // Calibration part in the EMG void is stopped 00272 ledC = 1; 00273 TijdEMGCal.stop(); 00274 TijdEMGCal.reset(); 00275 T1 = 0.35*Max; // Determination of thresholds 00276 T2 = 0.5*Max; 00277 T3 = 0.35*Max2; 00278 T4 = 0.5*Max2; 00279 wait (3); 00280 Excecute = true; 00281 } 00282 } 00283 00284 int main() 00285 { 00286 upperlimit = n1*twopi; // Calculation for limits of the robot 00287 turnlimit = n2*twopi; 00288 ledF = 1; 00289 ledC = 1; 00290 PowerMotor.write(0); 00291 PowerMotor2.write(0); 00292 MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers 00293 biquadTicker.attach(myController,1/Fs2); 00294 PowerServo.period_ms(20); 00295 while (true) { 00296 Calibration(); // Makes it able to calibrate during firing mode 00297 while (Excecute) { 00298 // Motor 1 00299 if (Rotatie >= upperlimit) { // If OutRange 00300 Goal = upperlimit - margin-0.2; 00301 OutRange = true; 00302 } 00303 if (Rotatie <= downlimit) { // If OutRange 00304 Goal = 0 + margin +0.2; 00305 OutRange = true; 00306 } 00307 if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange 00308 OutRange = false; 00309 } 00310 if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange 00311 if (y8 >= T2 ) { 00312 Direction = 1; 00313 v = 0.13; 00314 } 00315 if (y8 > T1 && y8 < T2) { 00316 Direction = 0; 00317 v = 0.07; 00318 } 00319 if (y8 <= T1) { 00320 Direction = 0; 00321 v = 0; 00322 } 00323 } 00324 00325 // Motor 00326 if (Rotatie2 >= turnlimit) { // If OutRange 00327 Goal2 = turnlimit-0.1; 00328 OutRange2 = true; 00329 } 00330 if (Rotatie2 <= -turnlimit) { // If OutRange 00331 Goal2 = -turnlimit+0.1; 00332 OutRange2 = true; 00333 } 00334 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange 00335 OutRange2 = false; 00336 } 00337 if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange 00338 if (y18 >= T4 ) { 00339 Direction2 = 1; 00340 v2 = 0.06; 00341 } 00342 if (y18 > T3 && y18 < T4) { 00343 Direction2 = 0; 00344 v2 = 0.06; 00345 } 00346 if (y18 <= T3) { 00347 Direction2 = 0; 00348 v2 = 0; 00349 } 00350 } 00351 if (Button2 >= 0.8) { // Firing Rubber Band Gun 00352 wait(0.2); 00353 ledF = 0; 00354 PowerServo.write(0.27); 00355 wait (1); 00356 PowerServo.write(0.04); 00357 wait (1); 00358 PowerServo.write(0.27); 00359 wait (1); 00360 PowerServo.write(0.04); 00361 ledF = 1; 00362 Fired=Fired+1; 00363 if (Fired == 3) { 00364 wait (1); 00365 Home = true; 00366 Excecute = false; 00367 } 00368 } 00369 if (Button3 == 0) { 00370 Excecute = false; 00371 Home = true; 00372 } 00373 Calibration(); 00374 } 00375 00376 while (Home) { // Return to base position 00377 OutRange = true; // Activation of PID control part 00378 OutRange2 = true; 00379 Goal = margin; 00380 Goal2 = 0; 00381 if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again 00382 timer.start(); 00383 } else { 00384 timer.stop(); 00385 timer.reset(); 00386 } 00387 if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset. 00388 Errori = 0; 00389 Errord = 0; 00390 Errorp = 0; 00391 Errori2 = 0; 00392 Errord2 = 0; 00393 Errorp2 = 0; 00394 Fired = 0; 00395 wait (5); 00396 Excecute = true; 00397 Home = false; 00398 } 00399 } 00400 } 00401 }
Generated on Thu Aug 18 2022 13:27:36 by 1.7.2