EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
Diff: main.cpp
- Revision:
- 23:c97e206cf2a7
- Parent:
- 22:2e1713475f5f
- Child:
- 24:711c7c388e57
diff -r 2e1713475f5f -r c97e206cf2a7 main.cpp --- a/main.cpp Mon Oct 12 18:23:05 2015 +0000 +++ b/main.cpp Wed Oct 14 14:55:21 2015 +0000 @@ -5,27 +5,25 @@ 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); +PwmOut PowerServo(D3); DigitalIn Button(PTC6); -DigitalIn Button2(PTC4); +DigitalIn Button2(PTA4); 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; +Ticker sampleEMG; Timer timer; HIDScope scope(3); -double emg_value +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; @@ -36,8 +34,10 @@ const double Ki = 0.2; double v = 0; //snelheid van de motor (0-0.1 double upperlimit; //max aantal rotaties +const double downlimit = 0.3; bool Excecute = false; bool Home = false; +bool OutRange = false; const double Fs=100; const double T1 = 0.33333; // Treshold 1 const double T2 = 0.66666; // Treshold 2 @@ -47,30 +47,29 @@ void MotorSet() { - - 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 (OutRange) { + 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; } - 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,emg_value); scope.send(); } @@ -79,7 +78,6 @@ // Lees het EMG signaal uit //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben emg_value = PotMeter.read(); - } @@ -91,71 +89,84 @@ 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); + sampleEMG.attach(EMGsample,0.5/Fs); + PowerServo.period_ms(20); while (true) { Encoder.reset(); if (Button == 0) { Excecute =! Excecute; } while (Excecute ) { //Dit wordt gebruikt voor motor 1 - if (Rotatie >= upperlimit) { + //pc.printf("Rotatie = %f \n", Rotatie); + //pc.printf("Out of Range = %i \n", OutRange); + if (Rotatie >= upperlimit-margin) { Goal = upperlimit - margin; + OutRange = true; } - if (Rotatie <= downlimit) { + if (Rotatie <= downlimit+margin) { Goal = 0 + margin; + OutRange = true; } - - else { + if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan + OutRange = false; // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden - if (emg_value => T2 ) { + if (emg_value >= T2 ) { Direction = 1; v = 1; } if (emg_value >= T1 && emg_value <= T2) { Direction = 0; v = 1; + } + 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; + OutRange = true; + } + if (Rotatie <= downlimit+margin) { + Goal = 0 + margin; + OutRange = true; + } + if (Rotatie >= margin && Rotatie <= (upperlimit - margin)) { // Zodra ik hem uit de Range stuur dan + OutRange = false; + // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden + if (emg_value >= T2 ) { + Direction = 0; + v = 1; + } + if (emg_value >= T1 && emg_value <= T2) { + Direction = 1; + v = 1; } else { Direction = 0; v = 0; } } - 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; - } } + if (Button2 == 0) { + PowerServo.write(0.27); + wait (1); + PowerServo.write(0.04); + wait (1); + Fire=Fire+1; + } + if (Fire == 3) { + wait (1); + Excecute = false; + Home = true; + } + + while (Home) { - Goal = 0.3; // Het doel waar hij naar toe moet + OutRange = false; + Goal = margin; // Het doel waar hij naar toe moet if (fabs(Error)<=0.0015) { timer.start(); } else {