Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 31:85d3b4db5e2b
- Parent:
- 30:37e778f27fce
- Child:
- 32:c2c80a2ca83d
diff -r 37e778f27fce -r 85d3b4db5e2b main.cpp --- a/main.cpp Tue Oct 20 12:03:30 2015 +0000 +++ b/main.cpp Tue Oct 20 13:25:36 2015 +0000 @@ -17,16 +17,16 @@ // Buttons & EMG (PotMeter) DigitalIn Button(PTC6); DigitalIn Button2(PTA4); -AnalogIn PotMeter(A0); -AnalogIn PotMeter2(A2); -//AnalogIn EMG(A0); -//AnalogIn Emg(A1); +AnalogIn EMG(A0); +AnalogIn EMG2(A1); // Tickers & timers + +Ticker biquadTicker; Ticker MotorWrite; Ticker Sender; -Ticker sampleEMG; Timer timer; +Timer TijdEMGCal; // Debugging Serial pc(USBTX, USBRX); @@ -84,10 +84,126 @@ bool Excecute = false; bool Home = false; +// Filter +double Fs2 = 500; // in Hz +const double TijdCal = 5; +double Max = 0; +double Max2 = 0; +bool Cali = false; + +double u1; +double y1; +double y2; +double y3; +double y4; +double y5; +double y6; +double y7; +double y8; + +double u10; +double y10; +double y12; +double y13; +double y14; +double y15; +double y16; +double y17; +double y18; + +double f1_v1=0,f1_v2=0; +double f2_v1=0,f2_v2=0; +double f3_v1=0,f3_v2=0; +double f4_v1=0,f4_v2=0; +double f5_v1=0,f5_v2=0; +double f6_v1=0,f6_v2=0; +double f7_v1=0,f7_v2=0; + +double f1_v3=0,f1_v4=0; +double f2_v3=0,f2_v4=0; +double f3_v3=0,f3_v4=0; +double f4_v3=0,f4_v4=0; +double f5_v3=0,f5_v4=0; +double f6_v3=0,f6_v4=0; +double f7_v3=0,f7_v4=0; + +// Notch +const double gainNotch3=0.969922; +const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000; +const double gainNotch4=0.975183; +const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000; +const double gainNotch5=0.993678; +const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000; + +// High pass +const double gainHP1=0.939472; +const double f1_a1=-0.87894202296,f1_a2=0.00000000,f1_b0=1.0000000,f1_b1=-1.00000000,f1_b2=0.000000000; +const double gainHP2=0.935820; +const double f2_a1=-1.86387364983,f2_a2=0.87941229211,f2_b0=1.0000000000,f2_b1=-2.0000000,f2_b2=1.0000000; + +// Low pass +const double gainLP6=0.000048; +const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000; +const double gainLP7=0.000048; +const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000; + // Voids voor berekeningen in het hoofdprogramma +double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) +{ + double v = u-a1*v1-a2*v2; + double y=b0*v+b1*v1+b2*v2; + v2=v1; + v1=v; + return y; +} + +void myController() +{ + // 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); + + // 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); + + // 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); + + // 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); + + // 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); + + // 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); + + if (Cali == true) { + if (y8 >= Max) { + Max = y8; + } + if (y18 >= Max2) { + Max2 = y18; + } + } +} + void MotorSet() { // Eerst motor 1 (translatie) @@ -131,42 +247,11 @@ void Send() { Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4200; + Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; - 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); scope.send(); } -void EMGsample() -{ - // Lees het EMG signaal uit - //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben - emg_value = PotMeter.read(); - emg_value2 = PotMeter2.read(); -} - -void Fire () -{ - PowerServo.write(0.27); - wait (1); - PowerServo.write(0.04); - Fired=Fired+1; - pc.printf("Fire = %i", Fired); - if (Fired == 3) { - wait (1); - Excecute = false; - Home = true; - } -} - - -// Calibratie moet nog worden uitgevoerd!!!! - int main() { @@ -175,17 +260,35 @@ pc.baud(115200); PowerMotor.write(0); PowerMotor2.write(0); - Sender.attach(Send,0.5/Fs); - MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde - sampleEMG.attach(EMGsample,0.5/Fs); + Sender.attach(Send,1/Fs); + 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); while (true) { Encoder.reset(); Encoder2.reset(); if (Button == 0) { - Excecute =! Excecute; + Cali = true; + TijdEMGCal.start(); } - while (Excecute ) { + 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; + } + while (Excecute) { // Eerst wordt motor 1 aangestuurd if (Rotatie >= upperlimit) { //Als hij buiten bereik is Goal = upperlimit - margin; @@ -240,7 +343,16 @@ } } if (Button2 == 0) { //Afvuren van de RBG - Fire(); + PowerServo.write(0.27); + wait (1); + PowerServo.write(0.04); + Fired=Fired+1; + pc.printf("Fire = %i", Fired); + if (Fired == 3) { + wait (1); + Home = true; + Excecute = false; + } } }