EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 32:c2c80a2ca83d
- Parent:
- 31:85d3b4db5e2b
- Child:
- 33:4e3870ab4e17
diff -r 85d3b4db5e2b -r c2c80a2ca83d main.cpp --- a/main.cpp Tue Oct 20 13:25:36 2015 +0000 +++ b/main.cpp Wed Oct 21 13:53:23 2015 +0000 @@ -14,9 +14,10 @@ QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); PwmOut PowerServo(D3); -// Buttons & EMG (PotMeter) +// Buttons & EMG DigitalIn Button(PTC6); DigitalIn Button2(PTA4); +DigitalIn Button3(D9); AnalogIn EMG(A0); AnalogIn EMG2(A1); @@ -31,6 +32,7 @@ // Debugging Serial pc(USBTX, USBRX); HIDScope scope(6); +DigitalOut led(LED_RED); @@ -40,10 +42,10 @@ int Fired = 0; // EMG -double emg_value; -double emg_value2; -const double T1 = 0.33333; // Treshold 1 -const double T2 = 0.66666; // Treshold 2 +double T1 = 0; +double T2 = 0; +double T3 = 0; +double T4 = 0; // Motor 1 (Translatie) double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) @@ -57,7 +59,7 @@ const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides const double Kd = 10; const double Ki = 0.2; -double v = 0; //snelheid van de motor (0-0.1)? +double v = 0; double upperlimit; //max aantal rotaties omhoog const double downlimit = 0.4; const double margin = 0.4; @@ -85,7 +87,7 @@ bool Home = false; // Filter -double Fs2 = 500; // in Hz +double Fs2 = 1000; // in Hz const double TijdCal = 5; double Max = 0; double Max2 = 0; @@ -165,34 +167,34 @@ // EMG 1 u1 = EMG.read(); // Notch - double y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); - double y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); - double y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); + y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); + y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); + y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); // HP - double y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); - double y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); + y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); + y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); // LP y6 = fabs(y5); - double y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); - double y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); + y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); + y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); // EMG 2 u10 = EMG2.read(); // Notch - double y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); - double y12 = biquad(y10,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); - double y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); + y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); + y12 = biquad(y10,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); + y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); // HP - double y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); - double y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); + y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); + y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); // LP y16 = fabs(y15); - double y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); - double y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); + y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); + y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); if (Cali == true) { if (y8 >= Max) { @@ -250,9 +252,43 @@ Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; + scope.set(0,Goal); + scope.set(1,Rotatie); + scope.set(2,y8); + scope.set(3,Goal2); + scope.set(4,Rotatie2); + scope.set(5,y18); scope.send(); } +void Calibration () +{ + if (OutRange == false && OutRange2 == false) { + if (Button == 0) { + Cali = true; + TijdEMGCal.start(); + Excecute = false; + led = 0; + v = 0; + v2 = 0; + } + } + if (TijdEMGCal.read() >= TijdCal) { + Cali = false; + led = 1; + TijdEMGCal.stop(); + TijdEMGCal.reset(); + T1 = 0.1*Max; + T2 = 0.25*Max; + T3 = 0.1*Max2; + T4 = 0.25*Max2; + pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2); + wait (3); + Excecute = true; + + } +} + int main() { upperlimit = n1*twopi; @@ -264,30 +300,9 @@ MotorWrite.attach(MotorSet,1/Fs); biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); - - scope.set(0,Goal); - scope.set(1,Rotatie); - scope.set(2,emg_value); - scope.set(3,Goal2); - scope.set(4,Rotatie2); - scope.set(5,emg_value2); + led = 1; while (true) { - Encoder.reset(); - Encoder2.reset(); - if (Button == 0) { - Cali = true; - TijdEMGCal.start(); - } - if (TijdEMGCal.read() >= TijdCal) { - Cali = false; - TijdEMGCal.stop(); - TijdEMGCal.reset(); - double T1 = 0.2*Max; - double T2 = 0.5*Max2; - pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2); - wait (3); - Excecute = true; - } + Calibration(); while (Excecute) { // Eerst wordt motor 1 aangestuurd if (Rotatie >= upperlimit) { //Als hij buiten bereik is @@ -302,15 +317,15 @@ OutRange = false; } if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing - if (emg_value >= T2 ) { + if (y8 >= T2 ) { Direction = 1; v = 0.1; } - if (emg_value > T1 && emg_value < T2) { + if (y8 > T1 && y8 < T2) { Direction = 0; v = 0.1; } - if (emg_value <= T1) { + if (y8 <= T1) { Direction = 0; v = 0; } @@ -329,15 +344,15 @@ OutRange2 = false; } if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing - if (emg_value2 >= T2 ) { + if (y18 >= T4 ) { Direction2 = 1; v2 = 0.1; } - if (emg_value2 > T1 && emg_value2 < T2) { + if (y18 > T3 && y18 < T4) { Direction2 = 0; v2 = 0.1; } - if (emg_value2 <= T1) { + if (y18 <= T3) { Direction2 = 0; v2 = 0; } @@ -354,6 +369,11 @@ Excecute = false; } } + if (Button3 == 0) { + Excecute = false; + Home = true; + } + Calibration(); } while (Home) { //Terugkeren naar vaste positie @@ -376,6 +396,10 @@ Errord2 = 0; Errorp2 = 0; Fired = 0; + wait (3); + Encoder.reset(); + Encoder2.reset(); + Excecute = true; } } }