Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 22:2e1713475f5f
- Parent:
- 21:d0156eadcbfe
- Child:
- 23:c97e206cf2a7
diff -r d0156eadcbfe -r 2e1713475f5f main.cpp --- a/main.cpp Mon Oct 12 13:44:24 2015 +0000 +++ b/main.cpp Mon Oct 12 18:23:05 2015 +0000 @@ -3,58 +3,87 @@ #include "math.h" #include "HIDScope.h" -DigitalOut Direction(D4); //1 = CCW - 0 = CW -PwmOut PowerMotor(D5); //van 0 tot 1 -PwmOut PowerServo(D7); -DigitalIn Button(PTC6); -DigitalIn Button2(PTC4); -AnalogIn PotMeter(A1); -QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder -Serial pc(USBTX, USBRX); -Ticker MotorWrite; -Ticker Sender; -Timer timer; -HIDScope scope(3); +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); +DigitalIn Button2(PTC4); +AnalogIn PotMeter(A1); +AnalogIn EMG(A0); +QEI Encoder(D13,D12,NC,32,QEI::X2_ENCODING); //Encoder +Serial pc(USBTX, USBRX); +Ticker MotorWrite; +Ticker Sender; +Ticker EMG; +Timer timer; +HIDScope scope(3); -double z=0; //initiele waarde potmeter -const double twopi = 6.2831853071795; -int Pulses; -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; -double Errord = 0; -double Errori = 0; -double Errorp = 0; -const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides -const double Kd = 10; -const double Ki = 0.2; -double v = 0; //snelheid van de motor (0-0.1 -double upperlimit; //max aantal rotaties -bool Excecute = false; -bool Excecute2 = false; -const double Fs=100; +double emg_value +const double twopi = 6.2831853071795; +int Pulses; +const double downlimit = 0.3; +const double margin = 0.3; +double Rotatie = 0; //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; +double Errord = 0; +double Errori = 0; +double Errorp = 0; +const double Kp = 0.2; //Moet berekend worden aan de hand van Control concept slides +const double Kd = 10; +const double Ki = 0.2; +double v = 0; //snelheid van de motor (0-0.1 +double upperlimit; //max aantal rotaties +bool Excecute = false; +bool Home = false; +const double Fs=100; +const double T1 = 0.33333; // Treshold 1 +const double T2 = 0.66666; // Treshold 2 +int Fire = 0; -double n = 3; +double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien void MotorSet() { - v=Kp*Error + Kd*Errord + Ki*Errori; + + 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; + } if (Error>=0) { Direction=1; } else { Direction=0; } + v=Kp*Error + Kd*Errord + Ki*Errori; PowerMotor.write(fabs(v)); } void Send() { + Pulses = Encoder.getPulses(); + Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien + Rotatieup = fmod(Rotatie,upperlimit); //Aantal radialen draaien binnen het bereik van upperlimit scope.set(0,Goal); scope.set(1,Rotatieup); - scope.set(2,Error); + scope.set(2,emg_value); scope.send(); } +void EMGsample() +{ + // Lees het EMG signaal uit + //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben + emg_value = PotMeter.read(); + +} + + + int main() { upperlimit = n*twopi; @@ -62,68 +91,83 @@ 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(EMGsample,0.5/Fs); while (true) { Encoder.reset(); if (Button == 0) { Excecute =! Excecute; } - while (Excecute ) { - 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.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; + while (Excecute ) { //Dit wordt gebruikt voor motor 1 + if (Rotatie >= upperlimit) { + Goal = upperlimit - margin; + } + if (Rotatie <= downlimit) { + Goal = 0 + margin; + } + + else { + // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden + if (emg_value => T2 ) { + Direction = 1; + v = 1; + } + if (emg_value >= T1 && emg_value <= T2) { + Direction = 0; + v = 1; + } else { + Direction = 0; + v = 0; + } } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); + while (Excecute) { // Dit is voor motor 2, dus naar boven en naar beneden + OutofRange = true if + if (Rotatie >= upperlimit) { + Goal = upperlimit - margin; + } + if (Rotatie <= downlimit) { + Goal = 0 + margin; + } + + else { + // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden + if (emg_value => T2 ) { + Direction = 1; + v = 1; + } + if (emg_value >= T1 && emg_value <= T2) { + Direction = 0; + v = 1; + } else { + Direction = 0; + v = 0; + } + } + } + if (Button2 = 0) { + // servo motor activation --> make a function of it + Fire=Fire+1; + } + if (Fire == 3) { + wait (1); + Excecute = false; + Home = true; + } + } + + while (Home) { + Goal = 0.3; // Het doel waar hij naar toe moet if (fabs(Error)<=0.0015) { timer.start(); } else { timer.stop(); timer.reset(); } - if (timer.read() >= 5) { + if (timer.read() >= 3) { Excecute=false; - Excecute2 = true; - Error = 0; - 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 - 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; - } - pc.printf("Error = %f\n Goal = %f\n", Error, Goal); - if (fabs(Error)<=0.0015) { - timer.start(); - } else { - timer.stop(); - timer.reset(); - } - if (timer.read() >= 5) { - Excecute=false; - Excecute2 = false; - Error = 0; + Home = false; Errori = 0; Errord = 0; } } } -} \ No newline at end of file +}