Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 27:9cca2ad74ec0
- Parent:
- 26:5b6c577fb3c1
- Child:
- 28:b7d01a55530f
--- a/main.cpp Thu Oct 15 15:08:49 2015 +0000 +++ b/main.cpp Thu Oct 15 16:03:40 2015 +0000 @@ -65,7 +65,6 @@ // Motor 2 (Rotatie) double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) - int Pulses2; double Rotatie2 = 0; double Goal2; @@ -91,11 +90,12 @@ void MotorSet() { + // Eerst motor 1 (translatie) if (OutRange) { Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal Errord = (Error-Errorp)/Fs; Errorp = Error; - if (fabs(Error) <= 0.1) { + if (fabs(Error) <= 0.5) { Errori = Errori + Error*1/Fs; } else { Errori = 0; @@ -109,28 +109,29 @@ } PowerMotor.write(fabs(v)); - //if (OutRange2) { -// Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal -// Errord2 = (Error2-Errorp2)/Fs; -// Errorp2 = Error2; -// if (fabs(Error2) <= 0.5) { -// Errori2 = Errori2 + Error2*1/Fs; -// } else { -// Errori2 = 0; -// } -// if (Error2>=0) { -// Direction=1; -// } else { -// Direction=0; -// } -// v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; -// } -// PowerMotor2.write(fabs(v2)); + // Dan motor 2 (rotatie) + if (OutRange2) { + Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal + Errord2 = (Error2-Errorp2)/Fs; + Errorp2 = Error2; + if (fabs(Error2) <= 0.5) { + Errori2 = Errori2 + Error2*1/Fs; + } else { + Errori2 = 0; + } + if (Error2>=0) { + Direction=1; + } else { + Direction=0; + } + v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; + } + PowerMotor2.write(fabs(v2)); } void Send() { Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien + Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; scope.set(0,Goal); @@ -181,17 +182,16 @@ if (Button == 0) { Excecute =! Excecute; } - pc.printf("dafuq"); while (Excecute ) { // Eerst wordt motor 1 aangestuurd - pc.printf("PotMeter = %f \n", PotMeter.read()); pc.printf("Rotatie = %f \n", Rotatie); + pc.printf("Rotatie2 = %f \n", Rotatie2); if (Rotatie >= upperlimit) { //Als hij buiten bereik is - Goal = upperlimit - margin-0.005; + Goal = upperlimit - margin; OutRange = true; } if (Rotatie <= downlimit) { //Als hij buiten bereik is - Goal = 0 + margin+0.05; + Goal = 0 + margin; OutRange = true; } if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is @@ -239,9 +239,9 @@ // v = 0; // } // } -// if (Button2 == 0) { //Afvuren van de RBG -// Fire(); -// } + if (Button2 == 0) { //Afvuren van de RBG + Fire(); + } } while (Home) { //Terugkeren naar vaste positie