EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
main.cpp
- Committer:
- Fernon
- Date:
- 2015-10-16
- Revision:
- 29:7653adbbb101
- Parent:
- 28:b7d01a55530f
- Child:
- 30:37e778f27fce
File content as of revision 29:7653adbbb101:
#include "mbed.h" #include "QEI.h" #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(D7); PwmOut PowerMotor2(D6); QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); PwmOut PowerServo(D3); // Buttons & EMG (PotMeter) DigitalIn Button(PTC6); DigitalIn Button2(PTA4); AnalogIn PotMeter(A0); 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(6); // Waardes const double twopi = 6.2831853071795; const double Fs=100; int Fired = 0; // EMG double emg_value; 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; double Rotatie = 0; //aantal graden dat de motor is gedraaid double Goal; //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 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 = 0.4; // 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; // Voids voor berekeningen in het hoofdprogramma void MotorSet() { // Eerst motor 1 (translatie) 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)); // Dan motor 2 (rotatie) 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) { Direction2 = 0; } else { Direction2 = 1; } v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2; } PowerMotor2.write(fabs(v2)); } void Send() { Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; scope.set(0,Goal); scope.set(1,Rotatie); scope.set(2,emg_value); scope.set(3,Goal2); scope.set(4,Rotatie2); scope.set(5,emg_value2); 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(); 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 = n1*twopi; turnlimit = n2*twopi; pc.baud(115200); PowerMotor.write(0); PowerMotor2.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(); Encoder2.reset(); if (Button == 0) { Excecute =! Excecute; } while (Excecute ) { // Eerst wordt motor 1 aangestuurd pc.printf("Rotatie = %f \n", Rotatie); pc.printf("Rotatie2 = %f \n", Rotatie2); if (Rotatie >= upperlimit) { //Als hij buiten bereik is Goal = upperlimit - margin; OutRange = true; } if (Rotatie <= downlimit) { //Als hij buiten bereik is Goal = 0 + margin; OutRange = true; } 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) { //EMG aansturing if (emg_value >= T2 ) { Direction = 1; v = 0.1; } if (emg_value > T1 && emg_value < T2) { Direction = 0; v = 0.1; } if (emg_value <= T1) { Direction = 0; v = 0; } } // 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 (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // Voor als hij voor het eerst weer binnen bereik is OutRange2 = false; } if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { // EMG aansturing if (emg_value2 >= T2 ) { Direction2 = 1; v2 = 0.05; } if (emg_value2 > T1 && emg_value2 < T2) { Direction2 = 0; v2 = 0.05; } if (emg_value2 <= T1) { Direction2 = 0; v2 = 0; } } if (Button2 == 0) { //Afvuren van de RBG Fire(); } } 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) { Home = false; Errori = 0; Errord = 0; Errorp = 0; Errori2 = 0; Errord2 = 0; Errorp2 = 0; } } } }