Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 33:4e3870ab4e17
- Parent:
- 32:c2c80a2ca83d
- Child:
- 34:67f211c08e74
--- a/main.cpp Wed Oct 21 13:53:23 2015 +0000 +++ b/main.cpp Tue Oct 27 15:33:48 2015 +0000 @@ -61,8 +61,8 @@ const double Ki = 0.2; double v = 0; double upperlimit; //max aantal rotaties omhoog -const double downlimit = 0.4; -const double margin = 0.4; +const double downlimit = 0.8; +const double margin = 0.8; bool OutRange = false; // Motor 2 (Rotatie) @@ -87,7 +87,7 @@ bool Home = false; // Filter -double Fs2 = 1000; // in Hz +double Fs2 = 500; // in Hz const double TijdCal = 5; double Max = 0; double Max2 = 0; @@ -224,6 +224,11 @@ Direction=0; } v=Kp*Error + Kd*Errord + Ki*Errori; + if (Home == true) { + if (v >0.15) { + v = 0.15; + } + } } PowerMotor.write(fabs(v)); @@ -252,12 +257,10 @@ Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; - scope.set(0,Goal); - scope.set(1,Rotatie); - scope.set(2,y8); - scope.set(3,Goal2); - scope.set(4,Rotatie2); - scope.set(5,y18); + scope.set(0,Rotatie); + scope.set(1,y8); + scope.set(2,Rotatie2); + scope.set(3,y18); scope.send(); } @@ -278,11 +281,11 @@ led = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); - T1 = 0.1*Max; - T2 = 0.25*Max; - T3 = 0.1*Max2; - T4 = 0.25*Max2; - pc.printf("Max = %f\nT1 = %f\nT2 = %f", Max, T1, T2); + 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); wait (3); Excecute = true; @@ -304,6 +307,7 @@ while (true) { 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; @@ -319,11 +323,11 @@ if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing if (y8 >= T2 ) { Direction = 1; - v = 0.1; + v = 0.15; } if (y8 > T1 && y8 < T2) { Direction = 0; - v = 0.1; + v = 0.15; } if (y8 <= T1) { Direction = 0; @@ -346,11 +350,11 @@ if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing if (y18 >= T4 ) { Direction2 = 1; - v2 = 0.1; + v2 = 0.05; } if (y18 > T3 && y18 < T4) { Direction2 = 0; - v2 = 0.1; + v2 = 0.05; } if (y18 <= T3) { Direction2 = 0; @@ -379,7 +383,7 @@ while (Home) { //Terugkeren naar vaste positie pc.printf("Home\n"); OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. - Goal = 0; + Goal = margin; Goal2 = 0; if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) { timer.start();