Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
Diff: main.cpp
- Revision:
- 25:230bd4e1f3ef
- Parent:
- 24:711c7c388e57
- Child:
- 26:5b6c577fb3c1
--- a/main.cpp Wed Oct 14 15:27:45 2015 +0000 +++ b/main.cpp Thu Oct 15 14:24:49 2015 +0000 @@ -3,28 +3,53 @@ #include "math.h" #include "HIDScope.h" + + +// Motor 1 & 2 DigitalOut Direction(D4); //1 = CCW - 0 = CW, moet nog omgezet worden naar up en down PwmOut PowerMotor(D5); //van 0 tot 1 +QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); //Encoder +DigitalOut Direction2(D6); +PwmOut PowerMotor2(D7); +QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); PwmOut PowerServo(D3); + +// Buttons & EMG (PotMeter) 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); +AnalogIn PotMeter2(A2); +//AnalogIn EMG(A0); +//AnalogIn Emg(A1); + +// Tickers & timers Ticker MotorWrite; Ticker Sender; Ticker sampleEMG; Timer timer; + +// Debugging +Serial pc(USBTX, USBRX); HIDScope scope(3); + +// Waardes +const double twopi = 6.2831853071795; +const double Fs=100; +int Fired = 0; + +// EMG double emg_value; -const double twopi = 6.2831853071795; +double emg_value2; +const double T1 = 0.33333; // Treshold 1 +const double T2 = 0.66666; // Treshold 2 + +// Motor 1 (Translatie) +double n1 = 3.861464193; // Aantal rondjes dat ons apparaat maximaal mag draaien (omhoog) 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 Goal; //initele waarde goal waar de motor naar toe moet, dit wordt gedaan double Error = 0; double Errord = 0; double Errori = 0; @@ -32,18 +57,37 @@ 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; +double v = 0; //snelheid van de motor (0-0.1)? +double upperlimit; //max aantal rotaties omhoog +const double downlimit = 0.4; +const double margin = 0.4; +bool OutRange = false; + +// Motor 2 (Rotatie) +double n2 = 0.3125; // Aantal rondjes dat dat ons apparaat maximaal mag draaien (rotatie) + +int Pulses2; +double Rotatie2 = 0; +double Goal2; +double Error2 = 0; +double Errord2 = 0; +double Errori2 = 0; +double Errorp2 = 0; +const double Kp2 = 0.2; +const double Kd2 = 10; +const double Ki2 = 0.2; +double v2 = 0; +double turnlimit; // max aantal rotaties voor roteren hele robot +// Margin 2 is in ons geval 0 +bool OutRange2 = false; + +// Activatie tussen het schietgedeelte en terugkeergedeelte 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 + +// Voids voor berekeningen in het hoofdprogramma void MotorSet() { @@ -51,7 +95,7 @@ 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) { + if (fabs(Error) <= 0.1) { Errori = Errori + Error*1/Fs; } else { Errori = 0; @@ -64,11 +108,31 @@ v=Kp*Error + Kd*Errord + Ki*Errori; } PowerMotor.write(fabs(v)); + + if (OutRange2) { + Error2 = Goal2-Rotatie2; // De error die het motortje maakt ten opzichte van je Goal + Errord2 = (Error2-Errorp2)/Fs; + Errorp2 = Error2; + if (fabs(Error2) <= 0.5) { + Errori2 = Errori2 + Error2*1/Fs; + } else { + Errori2 = 0; + } + if (Error2>=0) { + Direction=1; + } else { + Direction=0; + } + v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; + } + PowerMotor2.write(fabs(v2)); } void Send() { Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; // Aantal radialen draaien + Pulses2 = Encoder2.getPulses(); + Rotatie2 = (Pulses*twopi)/4200; scope.set(0,Goal); scope.set(1,Rotatie); scope.set(2,emg_value); @@ -79,13 +143,32 @@ // Lees het EMG signaal uit //emg_value = emg.read(); Deze moet toegepast worden als we EMG hebben emg_value = PotMeter.read(); + emg_value2 = PotMeter2.read(); } +void Fire () +{ + PowerServo.write(0.27); + wait (1); + PowerServo.write(0.04); + wait (1); + Fired=Fired+1; + if (Fired >= 3) { + wait (1); + Excecute = false; + Home = true; + Fired = 0; + } +} + + +// Calibratie moet nog worden uitgevoerd!!!! int main() { - upperlimit = n*twopi; + upperlimit = n1*twopi; + turnlimit = n2*twopi; pc.baud(115200); PowerMotor.write(0); Sender.attach(Send,0.5/Fs); @@ -94,25 +177,25 @@ PowerServo.period_ms(20); while (true) { Encoder.reset(); + Encoder2.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) { + + while (Excecute ) { + // Eerst wordt motor 1 aangestuurd + if (Rotatie >= upperlimit) { //Als hij buiten bereik is Goal = upperlimit - margin; OutRange = true; } - if (Rotatie <= downlimit) { + if (Rotatie <= downlimit) { //Als hij buiten bereik is Goal = 0 + margin; OutRange = true; } - if (Rotatie >= margin && Rotatie <= upperlimit - margin) { + if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // Voor als hij voor het eerst weer binnen bereik is OutRange = false; } - if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { - // PowerMotor.write(EMG), hier moet dus de output van het EMG signaal gebruikt worden + if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing if (emg_value >= T2 ) { Direction = 1; v = 1; @@ -126,62 +209,59 @@ 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; + // Vanaf hier wordt motor 2 aangestuurd + if (Rotatie2 >= turnlimit) { //Als hij buiten bereik is + Goal2 = turnlimit; + OutRange2 = true; + } + if (Rotatie2 <= -turnlimit) { //Als hij buiten bereik is + Goal2 = -turnlimit; + OutRange2 = true; } - if (Rotatie <= downlimit+margin) { - Goal = 0 + margin; - OutRange = true; + if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is + OutRange2 = false; } - 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 (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing + + if (emg_value2 >= T2 ) { + Direction = 1; + v = 1; + } + if (emg_value2 >= T1 && emg_value2 <= T2) { Direction = 0; v = 1; } - if (emg_value >= T1 && emg_value <= T2) { - Direction = 1; - v = 1; - } else { + if (emg_value2 <= T1) { Direction = 0; v = 0; } } + if (Button2 == 0) { //Afvuren van de RBG + Fire(); + } } - 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) { + while (Home) { //Terugkeren naar vaste positie + OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. + Goal = 0; + Goal2 = 0; + if (fabs(Error)<=0.0015 && fabs(Error2)<=0.0015) { timer.start(); } else { timer.stop(); timer.reset(); } if (timer.read() >= 3) { - Excecute=false; Home = false; Errori = 0; Errord = 0; + Errorp = 0; + Errori2 = 0; + Errord2 = 0; + Errorp2 = 0; + wait (10); + Excecute = true; } } }