Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp
- Committer:
- Fernon
- Date:
- 2015-10-14
- Revision:
- 24:711c7c388e57
- Parent:
- 23:c97e206cf2a7
- Child:
- 25:230bd4e1f3ef
File content as of revision 24:711c7c388e57:
#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(D3); DigitalIn Button(PTC6); 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 sampleEMG; Timer timer; HIDScope scope(3); double emg_value; const double twopi = 6.2831853071795; int Pulses; const double margin = 0.4; double Rotatie = 0; //aantal graden dat de motor is gedraaid 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 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 int Fire = 0; double n = 4.43546; // Aantal rondjes dat ons apparaat maximaal mag draaien void MotorSet() { 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; } PowerMotor.write(fabs(v)); } void Send() { Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien scope.set(0,Goal); scope.set(1,Rotatie); 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; 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 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 //pc.printf("Rotatie = %f \n", Rotatie); //pc.printf("Out of Range = %i \n", OutRange); if (Rotatie >= upperlimit) { Goal = upperlimit - margin; OutRange = true; } if (Rotatie <= downlimit) { Goal = 0 + margin; OutRange = true; } if (Rotatie >= margin && Rotatie <= upperlimit - margin) { OutRange = false; } if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { // 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; } 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; } } } 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) { OutRange = false; Goal = margin; // Het doel waar hij naar toe moet if (fabs(Error)<=0.0015) { timer.start(); } else { timer.stop(); timer.reset(); } if (timer.read() >= 3) { Excecute=false; Home = false; Errori = 0; Errord = 0; } } } }