Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 26:5b6c577fb3c1
- Parent:
- 25:230bd4e1f3ef
- Child:
- 27:9cca2ad74ec0
diff -r 230bd4e1f3ef -r 5b6c577fb3c1 main.cpp --- a/main.cpp Thu Oct 15 14:24:49 2015 +0000 +++ b/main.cpp Thu Oct 15 15:08:49 2015 +0000 @@ -109,30 +109,30 @@ } 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)); + //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 Pulses2 = Encoder2.getPulses(); - Rotatie2 = (Pulses*twopi)/4200; + Rotatie2 = (Pulses2*twopi)/4200; scope.set(0,Goal); scope.set(1,Rotatie); scope.set(2,emg_value); @@ -181,15 +181,17 @@ 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); if (Rotatie >= upperlimit) { //Als hij buiten bereik is - Goal = upperlimit - margin; + Goal = upperlimit - margin-0.005; OutRange = true; } if (Rotatie <= downlimit) { //Als hij buiten bereik is - Goal = 0 + margin; + Goal = 0 + margin+0.05; OutRange = true; } if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is @@ -210,36 +212,36 @@ } } - // Vanaf hier wordt motor 2 aangestuurd - if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is - Goal2 = turnlimit; - OutRange2 = true; - } - if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is - Goal2 = -turnlimit; - OutRange2 = true; - } - if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is - OutRange2 = false; - } - if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing - - if (emg_value2 >= T2 ) { - Direction = 1; - v = 1; - } - if (emg_value2 >= T1 && emg_value2 <= T2) { - Direction = 0; - v = 1; - } - if (emg_value2 <= T1) { - Direction = 0; - v = 0; - } - } - if (Button2 == 0) { //Afvuren van de RBG - Fire(); - } + //// Vanaf hier wordt motor 2 aangestuurd +// if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is +// Goal2 = turnlimit; +// OutRange2 = true; +// } +// if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is +// Goal2 = -turnlimit; +// OutRange2 = true; +// } +// if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is +// OutRange2 = false; +// } +// if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing +// +// if (emg_value2 >= T2 ) { +// Direction = 1; +// v = 1; +// } +// if (emg_value2 >= T1 && emg_value2 <= T2) { +// Direction = 0; +// v = 1; +// } +// if (emg_value2 <= T1) { +// Direction = 0; +// v = 0; +// } +// } +// if (Button2 == 0) { //Afvuren van de RBG +// Fire(); +// } } while (Home) { //Terugkeren naar vaste positie @@ -260,8 +262,6 @@ Errori2 = 0; Errord2 = 0; Errorp2 = 0; - wait (10); - Excecute = true; } } }