EMG driven robot which shoots elastic bands
Fork of ROBOBIRDS_FINAL by
main.cpp
- Committer:
- Fernon
- Date:
- 2015-10-27
- Revision:
- 34:67f211c08e74
- Parent:
- 33:4e3870ab4e17
- Child:
- 35:b71140a46b9c
File content as of revision 34:67f211c08e74:
#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 DigitalIn Button(PTC6); DigitalIn Button2(PTA4); DigitalIn Button3(D9); //AnalogIn EMG(A0); //AnalogIn EMG2(A1); AnalogIn EMG(A2); AnalogIn EMG2(A3); // Tickers & timers Ticker biquadTicker; Ticker MotorWrite; Ticker Sender; Timer timer; Timer TijdEMGCal; // Debugging Serial pc(USBTX, USBRX); HIDScope scope(6); DigitalOut led(LED_RED); // Waardes const double twopi = 6.2831853071795; const double Fs=100; int Fired = 0; // EMG double T1 = 0; double T2 = 0; double T3 = 0; double T4 = 0; // Motor 1 (Translatie) const 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; double upperlimit; //max aantal rotaties omhoog const double downlimit = 0.8; const double margin = 0.8; bool OutRange = false; // Motor 2 (Rotatie) const 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; // Filter double Fs2 = 500; // in Hz const double TijdCal = 5; double Max = 0; double Max2 = 0; bool Cali = false; double u1; double y1; double y2; double y3; double y4; double y5; double y6; double y7; double y8; double u10; double y10; double y12; double y13; double y14; double y15; double y16; double y17; double y18; double f1_v1=0,f1_v2=0; double f2_v1=0,f2_v2=0; double f3_v1=0,f3_v2=0; double f4_v1=0,f4_v2=0; double f5_v1=0,f5_v2=0; double f6_v1=0,f6_v2=0; double f7_v1=0,f7_v2=0; double f1_v3=0,f1_v4=0; double f2_v3=0,f2_v4=0; double f3_v3=0,f3_v4=0; double f4_v3=0,f4_v4=0; double f5_v3=0,f5_v4=0; double f6_v3=0,f6_v4=0; double f7_v3=0,f7_v4=0; // Notch const double gainNotch3=0.969922; const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000; const double gainNotch4=0.975183; const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000; const double gainNotch5=0.993678; const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000; // High pass const double gainHP1=0.939472; const double f1_a1=-0.87894202296,f1_a2=0.00000000,f1_b0=1.0000000,f1_b1=-1.00000000,f1_b2=0.000000000; const double gainHP2=0.935820; const double f2_a1=-1.86387364983,f2_a2=0.87941229211,f2_b0=1.0000000000,f2_b1=-2.0000000,f2_b2=1.0000000; // Low pass const double gainLP6=0.000048; const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000; const double gainLP7=0.000048; const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000; // Voids voor berekeningen in het hoofdprogramma double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2) { double v = u-a1*v1-a2*v2; double y=b0*v+b1*v1+b2*v2; v2=v1; v1=v; return y; } void myController() { // EMG 1 u1 = EMG.read(); // Notch y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); // HP y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); // LP y6 = fabs(y5); y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); // EMG 2 u10 = 0; //EMG2.read(); // Notch y10 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); y12 = biquad(y10,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4); y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5); // HP y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1); y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2); // LP y16 = fabs(y15); y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2); y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2); if (Cali == true) { if (y8 >= Max) { Max = y8; } if (y18 >= Max2) { Max2 = y18; } } } 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; if (Home == true) { if (v >0.15) { v = 0.15; } } } 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,Rotatie); scope.set(1,y8); scope.set(2,Rotatie2); scope.set(3,y18); scope.send(); } void Calibration () { if (OutRange == false && OutRange2 == false) { if (Button == 0) { Cali = true; TijdEMGCal.start(); Excecute = false; led = 0; v = 0; v2 = 0; } } if (TijdEMGCal.read() >= TijdCal) { Cali = false; led = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); T1 = 0.2*Max; T2 = 0.4*Max; T3 = 0.2*Max2; T4 = 0.4*Max2; pc.printf("T1 = %f\nT2 = %f\nT3 = %f\nT4 = %f\n", T1, T2, T3, T4); wait (3); Excecute = true; } } int main() { upperlimit = n1*twopi; turnlimit = n2*twopi; pc.baud(115200); PowerMotor.write(0); PowerMotor2.write(0); Sender.attach(Send,1/Fs); MotorWrite.attach(MotorSet,1/Fs); biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); led = 1; while (true) { Calibration(); while (Excecute) { pc.printf("Rotatie = %f \nRotatie2 = %f \n", Rotatie, Rotatie2); // Eerst wordt motor 1 aangestuurd 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 (y8 >= T2 ) { Direction = 1; v = 0.15; } if (y8 > T1 && y8 < T2) { Direction = 0; v = 0.15; } if (y8 <= 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 (y18 >= T4 ) { Direction2 = 1; v2 = 0.05; } if (y18 > T3 && y18 < T4) { Direction2 = 0; v2 = 0.05; } if (y18 <= T3) { Direction2 = 0; v2 = 0; } } if (Button2 == 0) { //Afvuren van de RBG PowerServo.write(0.27); wait (1); PowerServo.write(0.04); Fired=Fired+1; pc.printf("Fire = %i", Fired); if (Fired == 3) { wait (1); Home = true; Excecute = false; } } if (Button3 == 0) { Excecute = false; Home = true; } Calibration(); } while (Home) { //Terugkeren naar vaste positie pc.printf("Home\n"); OutRange = true; //Hiermee wordt het PID gedeelte van de motor control aangestuurd. Goal = margin; Goal2 = 0; if (fabs(Error)<=0.015 && fabs(Error2)<=0.015) { 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; Fired = 0; wait (3); Encoder.reset(); Encoder2.reset(); Excecute = true; } } } }