Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 21:d0156eadcbfe
- Parent:
- 20:473735947e52
- Child:
- 22:2e1713475f5f
diff -r 473735947e52 -r d0156eadcbfe main.cpp --- a/main.cpp Fri Oct 09 10:20:15 2015 +0000 +++ b/main.cpp Mon Oct 12 13:44:24 2015 +0000 @@ -3,7 +3,7 @@ #include "math.h" #include "HIDScope.h" -DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down +DigitalOut Direction(D4); //1 = CCW - 0 = CW PwmOut PowerMotor(D5); //van 0 tot 1 PwmOut PowerServo(D7); DigitalIn Button(PTC6); @@ -13,7 +13,6 @@ Serial pc(USBTX, USBRX); Ticker MotorWrite; Ticker Sender; -Ticker EMG; Timer timer; HIDScope scope(3); @@ -21,9 +20,7 @@ double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; int Pulses; -const double downlimit = 0.3 - const double margin = 0.3 - double Rotatie; //aantal graden dat de motor is gedraaid +double Rotatie; //aantal graden dat de motor is gedraaid double Rotatieup=0; //aantal graden dat de motor is gedraaid in een bereik van n*pi double Goal = 0; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; @@ -39,7 +36,7 @@ bool Excecute2 = false; const double Fs=100; -double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien +double n = 3; void MotorSet() { @@ -58,20 +55,6 @@ scope.set(2,Error); scope.send(); } -void EMGsignal() -{ - // Lees het EMG signaal uit - //if (Treshold => x) { -// v = 1 -// } else { -// v = 0 -// } -// PowerMotor.write(v) -} - - - - int main() { upperlimit = n*twopi; @@ -79,124 +62,46 @@ PowerMotor.write(0); Sender.attach(Send,0.5/Fs); MotorWrite.attach(MotorSet,1/Fs); // Deze ticker moet de waarde uitlezen van de PotMeter Fs keer per seconde - EMG.attach(EMGsignal,1/Fs); while (true) { Encoder.reset(); if (Button == 0) { Excecute =! Excecute; } - while (Excecute ) { //Dit wordt gebruikt voor motor 1 + while (Excecute ) { Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien - Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit + Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen + Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); - if (Rotatie >= upperlimit) { - Goal = upperlimit - margin; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + Goal = 0.5*twopi; //PotMeter.read()*upperlimit; // Het doel waar hij naar toe moet + Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal + Errord = (Error-Errorp)/Fs; + Errorp = Error; + if (fabs(Error) <= 0.3) { + Errori = Errori + Error*1/Fs; + } else { + Errori = 0; } - if (Rotatie <= downlimit) { - Goal = 0 + margin; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + if (fabs(Error)<=0.0015) { + timer.start(); + } else { + timer.stop(); + timer.reset(); } - - else { - // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden - Goal = z*upperlimit; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + if (timer.read() >= 5) { + Excecute=false; + Excecute2 = true; + Error = 0; + Errori = 0; + Errord = 0; } } - while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden - Pulses = Encoder.getPulses(); - Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien - Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit - pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); - if (Rotatie >= upperlimit) { - Goal = upperlimit - margin; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - } - if (Rotatie <= downlimit) { - Goal = 0 + margin; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - } - - else { - // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden - Goal = z*upperlimit; - Error = Goal-Rotatie; // De error die het motortje maakt ten opzichte van je Goal - Errord = (Error-Errorp)/Fs; - Errorp = Error; - if (fabs(Error) <= 0.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - } - } - - - -// Dit gedeelte moet vervangen worden if fired then Excecut2 = true after waiting 1 second - // if (fabs(Error)<=0.0015) { -// timer.start(); -// } else { -// timer.stop(); -// timer.reset(); -// } -// if (timer.read() >= 5) { -// Excecute=false; -// Excecute2 = true; -// Errori = 0; -// Errord = 0; -// } - -// } while (Excecute2) { Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4192; // Aantal graden draaien in radialen Rotatieup = fmod(Rotatie,upperlimit); //Aantal graden draaien in radialen binnen het bereik van upperlimit pc.printf ("Rotatie = %f [radialen] \n", Rotatieup); - Goal = 0.3; // Het doel waar hij naar toe moet + Goal = 0; // Het doel waar hij naar toe moet Error = Goal-Rotatieup; // De error die het motortje maakt ten opzichte van je Goal Errord = (Error-Errorp)/Fs; Errorp = Error; @@ -215,6 +120,7 @@ if (timer.read() >= 5) { Excecute=false; Excecute2 = false; + Error = 0; Errori = 0; Errord = 0; }