Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 38:eaf245d88d84
- Parent:
- 37:0dbf536a95c8
- Child:
- 39:e5bf4b1293fa
diff -r 0dbf536a95c8 -r eaf245d88d84 main.cpp --- a/main.cpp Wed Oct 28 14:22:23 2015 +0000 +++ b/main.cpp Thu Oct 29 09:34:20 2015 +0000 @@ -15,8 +15,8 @@ PwmOut PowerServo(D3); // Buttons & EMG -DigitalIn Button(PTC6); -DigitalIn Button2(PTA4); +AnalogIn Button(A3); // Calibration Button +AnalogIn Button2(A4); // Fire button DigitalIn Button3(D9); AnalogIn EMG(A0); AnalogIn EMG2(A1); @@ -32,7 +32,8 @@ // Debugging Serial pc(USBTX, USBRX); HIDScope scope(6); -DigitalOut led(LED_RED); +DigitalOut ledF(LED_RED); +DigitalOut ledC(LED_GREEN); @@ -48,7 +49,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.3; // 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 @@ -66,7 +67,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.4; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) int Pulses2; double Rotatie2 = 0; double Goal2; @@ -270,18 +271,19 @@ void Calibration () { if (OutRange == false && OutRange2 == false) { - if (Button == 0) { + if (Button >= 0.8) { + wait (0.2); Cali = true; TijdEMGCal.start(); Excecute = false; - led = 0; + ledC = 0; v = 0; v2 = 0; } } if (TijdEMGCal.read() >= TijdCal) { Cali = false; - led = 1; + ledC = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); T1 = 0.35*Max; @@ -296,8 +298,11 @@ int main() { + pc.printf("Welkom\n"); upperlimit = n1*twopi; turnlimit = n2*twopi; + ledF = 1; + ledC = 1; pc.baud(115200); PowerMotor.write(0); PowerMotor2.write(0); @@ -305,18 +310,18 @@ MotorWrite.attach(MotorSet,1/Fs); biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); - led = 1; while (true) { + pc.printf("grote While Loop\n"); Calibration(); while (Excecute) { pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2); // Eerst wordt motor 1 aangestuurd if (Rotatie >= upperlimit) { //Als hij buiten bereik is - Goal = upperlimit - margin; + Goal = upperlimit - margin-0.2; OutRange = true; } if (Rotatie <= downlimit) { //Als hij buiten bereik is - Goal = 0 + margin; + Goal = 0 + margin +0.2; OutRange = true; } if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is @@ -325,11 +330,11 @@ if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing if (y8 >= T2 ) { Direction = 1; - v = 0.12; + v = 0.13; } if (y8 > T1 && y8 < T2) { Direction = 0; - v = 0.08; + v = 0.07; } if (y8 <= T1) { Direction = 0; @@ -339,11 +344,11 @@ // Vanaf hier wordt motor 2 aangestuurd if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is - Goal2 = turnlimit; + Goal2 = turnlimit-0.1; OutRange2 = true; } if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is - Goal2 = -turnlimit; + Goal2 = -turnlimit+0.1; OutRange2 = true; } if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is @@ -352,21 +357,24 @@ if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing if (y18 >= T4 ) { Direction2 = 1; - v2 = 0.08; + v2 = 0.07; } if (y18 > T3 && y18 < T4) { Direction2 = 0; - v2 = 0.08; + v2 = 0.07; } if (y18 <= T3) { Direction2 = 0; v2 = 0; } } - if (Button2 == 0) { //Afvuren van de RBG + if (Button2 >= 0.8) { //Afvuren van de RBG + wait(0.2); + ledF = 0; PowerServo.write(0.27); wait (1); PowerServo.write(0.04); + ledF = 1; Fired=Fired+1; pc.printf("Fire = %i", Fired); if (Fired == 4) { @@ -402,7 +410,7 @@ Errord2 = 0; Errorp2 = 0; Fired = 0; - wait (3); + wait (5); Excecute = true; Home = false; }