Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 24:711c7c388e57
- Parent:
- 23:c97e206cf2a7
- Child:
- 25:230bd4e1f3ef
diff -r c97e206cf2a7 -r 711c7c388e57 main.cpp --- a/main.cpp Wed Oct 14 14:55:21 2015 +0000 +++ b/main.cpp Wed Oct 14 15:27:45 2015 +0000 @@ -22,7 +22,7 @@ double emg_value; const double twopi = 6.2831853071795; int Pulses; -const double margin = 0.3; +const double margin = 0.4; double Rotatie = 0; //aantal graden dat de motor is gedraaid double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; @@ -70,6 +70,7 @@ Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien scope.set(0,Goal); + scope.set(1,Rotatie); scope.set(2,emg_value); scope.send(); } @@ -99,16 +100,18 @@ while (Excecute ) { //Dit wordt gebruikt voor motor 1 //pc.printf("Rotatie = %f \n", Rotatie); //pc.printf("Out of Range = %i \n", OutRange); - if (Rotatie >= upperlimit-margin) { + if (Rotatie >= upperlimit) { Goal = upperlimit - margin; OutRange = true; } - if (Rotatie <= downlimit+margin) { + if (Rotatie <= downlimit) { Goal = 0 + margin; OutRange = true; } - if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan + if (Rotatie >= margin && Rotatie <= upperlimit - margin) { OutRange = false; + } + if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden if (emg_value >= T2 ) { Direction = 1; @@ -117,13 +120,14 @@ if (emg_value >= T1 && emg_value <= T2) { Direction = 0; v = 1; - } - if (emg_value <= T1){ + } + if (emg_value <= T1) { Direction = 0; v = 0; } } } + while (false ) { //Dit wordt gebruikt voor motor 2 MOET NOG OMGEZET WORDEN NAAR MOTOR 2!!!! if (Rotatie >= upperlimit-margin) { Goal = upperlimit - margin;