Fernon Eijkhoudt
/
ROBOBIRDS_FINAL
EMG driven robot which shoots elastic bands
Fork of RoboBirdV1 by
main.cpp
- Committer:
- Fernon
- Date:
- 2015-10-29
- Revision:
- 40:cac08f589732
- Parent:
- 39:e5bf4b1293fa
File content as of revision 40:cac08f589732:
#include "mbed.h" #include "QEI.h" #include "math.h" // Motor 1 & 2 DigitalOut Direction(D4); PwmOut PowerMotor(D5); QEI Encoder(D10,D11,NC,32,QEI::X2_ENCODING); DigitalOut Direction2(D7); PwmOut PowerMotor2(D6); QEI Encoder2(D12,D13,NC,32,QEI::X2_ENCODING); PwmOut PowerServo(D3); // Buttons & EMG AnalogIn Button(A3); // Calibration button AnalogIn Button2(A4); // Fire button DigitalIn Button3(D9); // Home button AnalogIn EMG(A0); AnalogIn EMG2(A1); // Tickers & timers Ticker biquadTicker; Ticker MotorWrite; Timer timer; Timer TijdEMGCal; // LED DigitalOut ledF(LED_RED); DigitalOut ledC(LED_GREEN); // Values const double twopi = 6.2831853071795; const double Fs=100; // Sample Frequency int Fired = 0; // Motor 1 (Translation) const double n1 = 3.3; // The amount of the rotations that is allowed int Pulses; double Rotatie = 0; //Rotation in radial double Goal; // Setpoint for Motor double Error = 0; // Setting initial values double Errord = 0; double Errori = 0; double Errorp = 0; const double Kp = 0.2; // PID controll parameters const double Kd = 10; const double Ki = 0.2; double v = 0; double upperlimit; //Maximum rounds up const double downlimit = 0.8; //Lowest limit of the robot const double margin = 0.8; //Margin for safe range bool OutRange = false; //In Range or not // Motor 2 (Rotation) const double n2 = 0.334; 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; // Margin 2 = 0 bool OutRange2 = false; // Switch between firing mode (Excecute) and return to Home mode bool Excecute = false; bool Home = false; // Filter double Fs2 = 500; // Frequency in Hz const double TijdCal = 5; // Time for calibration double Max = 0; // Max value for EMG double Max2 = 0; // Max value for EMG2 bool Cali = false; // Switch for Calibration double T1 = 0; // Thresholds for EMG double T2 = 0; double T3 = 0; double T4 = 0; double u1; //All variables for filtering double y1; double y2; double y3; double y4; double y5; double y6; double y7; double y8; double y9; double y10; double u10; double y11; 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.96860379641660377; const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.000000000; const double gainHP2=0.935820; const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.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 for calculations in the main programme 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 = EMG2.read(); // Notch y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3); y12 = biquad(y11,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() { // Calculation for position of motor Pulses = Encoder.getPulses(); Rotatie = (Pulses*twopi)/4200; Pulses2 = Encoder2.getPulses(); Rotatie2 = (Pulses2*twopi)/4200; // Motor 1 (translation) if (OutRange) { //If the Robot is OutRange or is going Home Error = Goal-Rotatie; Errord = (Error-Errorp)/Fs; Errorp = Error; if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small Errori = Errori + Error*1/Fs; } else { Errori = 0; } if (Error>=0) { // Determine the direction Direction=1; } else { Direction=0; } v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed if (Home == true) { //Maximum speed when OutRange if (v >0.15) { v = 0.15; } } } PowerMotor.write(fabs(v)); // Dan motor 2 (rotatie) if (OutRange2) { Error2 = Goal2-Rotatie2; 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 Calibration () { if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range if (Button >= 0.8) { wait (0.2); Cali = true; // Calibration part in the EMG void is being activated TijdEMGCal.start(); Excecute = false; ledC = 0; v = 0; v2 = 0; } } if (TijdEMGCal.read() >= TijdCal) { Cali = false; // Calibration part in the EMG void is stopped ledC = 1; TijdEMGCal.stop(); TijdEMGCal.reset(); T1 = 0.35*Max; // Determination of thresholds T2 = 0.5*Max; T3 = 0.35*Max2; T4 = 0.5*Max2; wait (3); Excecute = true; } } int main() { upperlimit = n1*twopi; // Calculation for limits of the robot turnlimit = n2*twopi; ledF = 1; ledC = 1; PowerMotor.write(0); PowerMotor2.write(0); MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers biquadTicker.attach(myController,1/Fs2); PowerServo.period_ms(20); while (true) { Calibration(); // Makes it able to calibrate during firing mode while (Excecute) { // Motor 1 if (Rotatie >= upperlimit) { // If OutRange Goal = upperlimit - margin-0.2; OutRange = true; } if (Rotatie <= downlimit) { // If OutRange Goal = 0 + margin +0.2; OutRange = true; } if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange OutRange = false; } if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange if (y8 >= T2 ) { Direction = 1; v = 0.13; } if (y8 > T1 && y8 < T2) { Direction = 0; v = 0.07; } if (y8 <= T1) { Direction = 0; v = 0; } } // Motor if (Rotatie2 >= turnlimit) { // If OutRange Goal2 = turnlimit-0.1; OutRange2 = true; } if (Rotatie2 <= -turnlimit) { // If OutRange Goal2 = -turnlimit+0.1; OutRange2 = true; } if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange OutRange2 = false; } if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange if (y18 >= T4 ) { Direction2 = 1; v2 = 0.06; } if (y18 > T3 && y18 < T4) { Direction2 = 0; v2 = 0.06; } if (y18 <= T3) { Direction2 = 0; v2 = 0; } } if (Button2 >= 0.8) { // Firing Rubber Band Gun wait(0.2); ledF = 0; PowerServo.write(0.27); wait (1); PowerServo.write(0.04); wait (1); PowerServo.write(0.27); wait (1); PowerServo.write(0.04); ledF = 1; Fired=Fired+1; if (Fired == 3) { wait (1); Home = true; Excecute = false; } } if (Button3 == 0) { Excecute = false; Home = true; } Calibration(); } while (Home) { // Return to base position OutRange = true; // Activation of PID control part OutRange2 = true; Goal = margin; Goal2 = 0; if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again timer.start(); } else { timer.stop(); timer.reset(); } if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset. Errori = 0; Errord = 0; Errorp = 0; Errori2 = 0; Errord2 = 0; Errorp2 = 0; Fired = 0; wait (5); Excecute = true; Home = false; } } } }