Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp
- Committer:
- Fernon
- Date:
- 2015-10-09
- Revision:
- 20:473735947e52
- Parent:
- 19:b8d959e02e5d
- Child:
- 21:d0156eadcbfe
File content as of revision 20:473735947e52:
#include "mbed.h" #include "QEI.h" #include "math.h" #include "HIDScope.h" 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); 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; 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; 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 n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien void MotorSet() { v=Kp*Error + Kd*Errord + Ki*Errori; if (Error>=0) { Direction=1; } else { Direction=0; } PowerMotor.write(fabs(v)); } void Send() { scope.set(0,Goal); scope.set(1,Rotatieup); 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; pc.baud(115200); 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 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); } } 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 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; Errori = 0; Errord = 0; } } } }