Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 36:5d1225d72bca
- Parent:
- 35:b71140a46b9c
- Child:
- 37:0dbf536a95c8
--- a/main.cpp Tue Oct 27 16:38:47 2015 +0000 +++ b/main.cpp Wed Oct 28 14:11:09 2015 +0000 @@ -18,10 +18,8 @@ DigitalIn Button(PTC6); DigitalIn Button2(PTA4); DigitalIn Button3(D9); -//AnalogIn EMG(A0); -//AnalogIn EMG2(A1); -AnalogIn EMG(A2); -AnalogIn EMG2(A3); +AnalogIn EMG(A0); +AnalogIn EMG2(A1); // Tickers & timers @@ -50,7 +48,7 @@ double T4 = 0; // Motor 1 (Translatie) -const double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) +const double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) int Pulses; double Rotatie = 0; //aantal graden dat de motor is gedraaid double Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan @@ -68,7 +66,7 @@ bool OutRange = false; // Motor 2 (Rotatie) -const double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) +const double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) int Pulses2; double Rotatie2 = 0; double Goal2; @@ -104,9 +102,11 @@ double y6; double y7; double y8; +double y9; +double y10; double u10; -double y10; +double y11; double y12; double y13; double y14; @@ -140,10 +140,10 @@ 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 gainHP1=0.96860379641660377; +const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.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; +const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000; // Low pass const double gainLP6=0.000048; @@ -186,8 +186,8 @@ // EMG 2 u10 = EMG2.read(); // Notch - 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); + y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); + y12 = biquad(y11,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 @@ -198,7 +198,6 @@ y16 = fabs(y15); 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); - y18 = EMG.read(); if (Cali == true) { if (y8 >= Max) { @@ -286,11 +285,11 @@ led = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); - T1 = 0.2*Max; - T2 = 0.4*Max; - T3 = 0.2*Max2; - T4 = 0.4*Max2; - pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4); + T1 = 0.35*Max; + T2 = 0.6*Max; + T3 = 0.35*Max2; + T4 = 0.6*Max2; + //pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4); wait (3); Excecute = true; } @@ -327,11 +326,11 @@ if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing if (y8 >= T2 ) { Direction = 1; - v = 0.15; + v = 0.12; } if (y8 > T1 && y8 < T2) { Direction = 0; - v = 0.15; + v = 0.08; } if (y8 <= T1) { Direction = 0; @@ -371,7 +370,7 @@ PowerServo.write(0.04); Fired=Fired+1; pc.printf("Fire = %i", Fired); - if (Fired == 3) { + if (Fired == 4) { wait (1); Home = true; Excecute = false; @@ -387,9 +386,10 @@ while (Home) { //Terugkeren naar vaste positie pc.printf("Home\n"); OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. + OutRange2 = true; Goal = margin; Goal2 = 0; - if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) { + if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { timer.start(); } else { timer.stop();