EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 20:473735947e52
- Parent:
- 19:b8d959e02e5d
- Child:
- 21:d0156eadcbfe
--- a/main.cpp Wed Oct 07 20:22:21 2015 +0000 +++ b/main.cpp Fri Oct 09 10:20:15 2015 +0000 @@ -3,7 +3,7 @@ #include "math.h" #include "HIDScope.h" -DigitalOut Direction(D4); //1 = CCW - 0 = CW +DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down PwmOut PowerMotor(D5); //van 0 tot 1 PwmOut PowerServo(D7); DigitalIn Button(PTC6); @@ -13,6 +13,7 @@ Serial pc(USBTX, USBRX); Ticker MotorWrite; Ticker Sender; +Ticker EMG; Timer timer; HIDScope scope(3); @@ -20,7 +21,9 @@ double z=0; //initiele waarde potmeter const double twopi = 6.2831853071795; int Pulses; -double Rotatie; //aantal graden dat de motor is gedraaid +const double downlimit = 0.3 + const double margin = 0.3 + 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; @@ -36,7 +39,7 @@ bool Excecute2 = false; const double Fs=100; -double n = 3; +double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien void MotorSet() { @@ -55,6 +58,20 @@ 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; @@ -62,46 +79,124 @@ 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 ) { + while (Excecute ) { //Dit wordt gebruikt voor motor 1 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 + 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); - 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.5) { - Errori = Errori + Error*1/Fs; - } else { - Errori = 0; + 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); } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - if (fabs(Error)<=0.0015) { - timer.start(); - } else { - timer.stop(); - timer.reset(); + 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); } - if (timer.read() >= 5) { - Excecute=false; - Excecute2 = true; - Error = 0; - Errori = 0; - Errord = 0; + + 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); } } + 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; // Het doel waar hij naar toe moet + Goal = 0.3; // 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; @@ -120,7 +215,6 @@ if (timer.read() >= 5) { Excecute=false; Excecute2 = false; - Error = 0; Errori = 0; Errord = 0; }