main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 #include "math.h"
00004 
00005 
00006 // Motor 1 & 2
00007 DigitalOut      Direction(D4); 
00008 PwmOut          PowerMotor(D5); 
00009 QEI             Encoder(D10,D11,NC,32,QEI::X2_ENCODING); 
00010 DigitalOut      Direction2(D7);
00011 PwmOut          PowerMotor2(D6);
00012 QEI             Encoder2(D12,D13,NC,32,QEI::X2_ENCODING);
00013 PwmOut          PowerServo(D3);
00014 
00015 // Buttons & EMG
00016 AnalogIn        Button(A3); // Calibration button  
00017 AnalogIn        Button2(A4); // Fire button
00018 DigitalIn       Button3(D9); // Home button
00019 AnalogIn        EMG(A0);
00020 AnalogIn        EMG2(A1);
00021 
00022 // Tickers & timers
00023 
00024 Ticker          biquadTicker;
00025 Ticker          MotorWrite;
00026 Timer           timer;
00027 Timer           TijdEMGCal;
00028 
00029 // LED
00030 DigitalOut      ledF(LED_RED);
00031 DigitalOut      ledC(LED_GREEN);
00032 
00033 
00034 
00035 // Values
00036 const double    twopi = 6.2831853071795;
00037 const double    Fs=100; // Sample Frequency
00038 int             Fired = 0;
00039 
00040 // Motor 1 (Translation)
00041 const double    n1 = 3.3; // The amount of the rotations that is allowed
00042 int             Pulses;
00043 double          Rotatie = 0; //Rotation in radial
00044 double          Goal; // Setpoint for Motor 
00045 double          Error = 0; // Setting initial values
00046 double          Errord = 0;
00047 double          Errori = 0;
00048 double          Errorp = 0;
00049 const double    Kp = 0.2; // PID controll parameters
00050 const double    Kd = 10;
00051 const double    Ki = 0.2;
00052 double          v = 0;
00053 double          upperlimit; //Maximum rounds up
00054 const double    downlimit = 0.8; //Lowest limit of the robot
00055 const double    margin = 0.8; //Margin for safe range
00056 bool            OutRange = false; //In Range or not
00057 
00058 // Motor 2 (Rotation)
00059 const double    n2 = 0.334;
00060 int             Pulses2;
00061 double          Rotatie2 = 0;
00062 double          Goal2;
00063 double          Error2 = 0;
00064 double          Errord2 = 0;
00065 double          Errori2 = 0;
00066 double          Errorp2 = 0;
00067 const double    Kp2 = 0.2;
00068 const double    Kd2 = 10;
00069 const double    Ki2 = 0.2;
00070 double          v2 = 0;
00071 double          turnlimit = 0.4;
00072 // Margin 2 = 0
00073 bool            OutRange2 = false;
00074 
00075 // Switch between firing mode (Excecute) and return to Home mode
00076 bool            Excecute = false;
00077 bool            Home = false;
00078 
00079 // Filter
00080 double          Fs2 = 500; // Frequency in Hz
00081 const double    TijdCal = 5; // Time for calibration
00082 double          Max = 0; // Max value for EMG
00083 double          Max2 = 0; // Max value for EMG2
00084 bool            Cali = false; // Switch for Calibration
00085 double          T1 = 0; // Thresholds for EMG
00086 double          T2 = 0;
00087 double          T3 = 0;
00088 double          T4 = 0;
00089 
00090 double u1; //All variables for filtering
00091 double y1;
00092 double y2;
00093 double y3;
00094 double y4;
00095 double y5;
00096 double y6;
00097 double y7;
00098 double y8;
00099 double y9;
00100 double y10;
00101 
00102 double u10;
00103 double y11;
00104 double y12;
00105 double y13;
00106 double y14;
00107 double y15;
00108 double y16;
00109 double y17;
00110 double y18;
00111 
00112 double f1_v1=0,f1_v2=0;
00113 double f2_v1=0,f2_v2=0;
00114 double f3_v1=0,f3_v2=0;
00115 double f4_v1=0,f4_v2=0;
00116 double f5_v1=0,f5_v2=0;
00117 double f6_v1=0,f6_v2=0;
00118 double f7_v1=0,f7_v2=0;
00119 
00120 double f1_v3=0,f1_v4=0;
00121 double f2_v3=0,f2_v4=0;
00122 double f3_v3=0,f3_v4=0;
00123 double f4_v3=0,f4_v4=0;
00124 double f5_v3=0,f5_v4=0;
00125 double f6_v3=0,f6_v4=0;
00126 double f7_v3=0,f7_v4=0;
00127 
00128 // Notch
00129 const double gainNotch3=0.969922;
00130 const double f3_a1=-1.56143694016, f3_a2=0.93984421899, f3_b0=1.00000000000, f3_b1=-1.60985807508, f3_b2=1.00000000000;
00131 const double gainNotch4=0.975183;
00132 const double f4_a1=-1.55188488157,f4_a2=0.96839115647,f4_b0=1.000000000,f4_b1=-1.60985807508,f4_b2=1.00000000;
00133 const double gainNotch5=0.993678;
00134 const double f5_a1=-1.61645491476,f5_a2=0.97057916088,f5_b0=1.000000000,f5_b1=-1.60985807508,f5_b2=1.00000000;
00135 
00136 // High pass
00137 const double gainHP1=0.96860379641660377;
00138 const double f1_a1=-1.9352943868599917,f1_a2=0.96960379641660377,f1_b0=1.0000000,f1_b1=-2.00000000,f1_b2=1.000000000;
00139 const double gainHP2=0.935820;
00140 const double f2_a1=-0.939062508174924,f2_a2=0,f2_b0=1.0000000000,f2_b1=-1.0000000,f2_b2=0.0000000;
00141 
00142 // Low pass
00143 const double gainLP6=0.000048;
00144 const double f6_a1=-1.97326192076 , f6_a2=0.97345330126 , f6_b0=1.0000000 , f6_b1=2.0000000 , f6_b2=1.0000000;
00145 const double gainLP7=0.000048;
00146 const double f7_a1=-1.98030504048 , f7_a2=0.98049710408 , f7_b0=1.0000000 , f7_b1=2.0000000 , f7_b2=1.0000000;
00147 
00148 
00149 
00150 // Voids for calculations in the main programme
00151 
00152 double biquad(double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)
00153 {
00154     double v = u-a1*v1-a2*v2;
00155     double y=b0*v+b1*v1+b2*v2;
00156     v2=v1;
00157     v1=v;
00158     return y;
00159 }
00160 
00161 void myController()
00162 {
00163     // EMG 1
00164     u1 = EMG.read();
00165     // Notch
00166     y1 = biquad(u1,f3_v1,f3_v2,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
00167     y2 = biquad(y1,f4_v1,f4_v2,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
00168     y3 = biquad(y2,f5_v1,f5_v2,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
00169 
00170     // HP
00171     y4 = biquad(y3,f1_v1,f1_v2,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
00172     y5 = biquad(y4,f2_v1,f2_v2,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
00173 
00174     // LP
00175     y6 = fabs(y5);
00176     y7 = biquad(y6,f6_v1,f6_v2,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
00177     y8 = biquad(y7,f7_v1,f7_v2,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
00178     
00179     
00180     // EMG 2
00181     u10 = EMG2.read();
00182     // Notch
00183     y11 = biquad(u10,f3_v3,f3_v4,f3_a1,f3_a2,f3_b0*gainNotch3,f3_b1*gainNotch3,f3_b2*gainNotch3);
00184     y12 = biquad(y11,f4_v3,f4_v4,f4_a1,f4_a2,f4_b0*gainNotch4,f4_b1*gainNotch4,f4_b2*gainNotch4);
00185     y13 = biquad(y12,f5_v3,f5_v4,f5_a1,f5_a2,f5_b0*gainNotch5,f5_b1*gainNotch5,f5_b2*gainNotch5);
00186 
00187     // HP
00188     y14 = biquad(y13,f1_v3,f1_v4,f1_a1,f1_a2,f1_b0*gainHP1,f1_b1*gainHP1,f1_b2*gainHP1);
00189     y15 = biquad(y14,f2_v3,f2_v4,f2_a1,f2_a2,f2_b0*gainHP2,f2_b1*gainHP2,f2_b2*gainHP2);
00190 
00191     // LP
00192     y16 = fabs(y15);
00193     y17 = biquad(y16,f6_v3,f6_v4,f6_a1,f6_a2,gainLP6*f6_b0,gainLP6*f6_b1,gainLP6*f6_b2);
00194     y18 = biquad(y17,f7_v3,f7_v4,f7_a1,f7_a2,gainLP7*f7_b0,gainLP7*f7_b1,gainLP7*f7_b2);
00195 
00196     if (Cali == true) {
00197         if (y8 >= Max) {
00198             Max = y8;
00199         }
00200         if (y18 >= Max2) {
00201             Max2 = y18;
00202         }
00203     }
00204 }
00205 
00206 void MotorSet()
00207 {
00208     // Calculation for position of motor
00209     Pulses = Encoder.getPulses();
00210     Rotatie = (Pulses*twopi)/4200;
00211     Pulses2 = Encoder2.getPulses();
00212     Rotatie2 = (Pulses2*twopi)/4200;
00213     // Motor 1 (translation)
00214     if (OutRange) { //If the Robot is OutRange or is going Home
00215         Error = Goal-Rotatie; 
00216         Errord = (Error-Errorp)/Fs;
00217         Errorp = Error;
00218         if (fabs(Error) <= 0.5) { // Only activate the I part of PID when the Error is small
00219             Errori = Errori + Error*1/Fs;
00220         } else {
00221             Errori = 0;
00222         }
00223         if (Error>=0) { // Determine the direction
00224             Direction=1;
00225         } else {
00226             Direction=0;
00227         }
00228         v=Kp*Error + Kd*Errord + Ki*Errori; //Calculation of speed
00229          if (Home == true) { //Maximum speed when OutRange
00230             if (v >0.15) {
00231                 v = 0.15;
00232             }
00233         }
00234     }
00235     PowerMotor.write(fabs(v));
00236 
00237     // Dan motor 2 (rotatie)
00238     if (OutRange2) {
00239         Error2 = Goal2-Rotatie2; 
00240         Errord2 = (Error2-Errorp2)/Fs;
00241         Errorp2 = Error2;
00242         if (fabs(Error2) <= 0.5) {
00243             Errori2 = Errori2 + Error2*1/Fs;
00244         } else {
00245             Errori2 = 0;
00246         }
00247         if (Error2>=0) {
00248             Direction2 = 0;
00249         } else {
00250             Direction2 = 1;
00251         }
00252         v2=Kp2*Error2 + Kd2*Errord2 + Ki2*Errori2;
00253     }
00254     PowerMotor2.write(fabs(v2));
00255 }
00256 
00257 void Calibration ()
00258 {
00259     if (OutRange == false && OutRange2 == false) { //Only allow callibration during firing mode and when in Range
00260         if (Button >= 0.8) {
00261             wait (0.2);
00262             Cali = true; // Calibration part in the EMG void is being activated
00263             TijdEMGCal.start();
00264             Excecute = false;
00265             ledC = 0;
00266             v = 0;
00267             v2 = 0;
00268         }
00269     }
00270     if (TijdEMGCal.read() >= TijdCal) {
00271         Cali = false; // Calibration part in the EMG void is stopped
00272         ledC = 1;
00273         TijdEMGCal.stop();
00274         TijdEMGCal.reset();
00275         T1 = 0.35*Max; // Determination of thresholds
00276         T2 = 0.5*Max;
00277         T3 = 0.35*Max2;
00278         T4 = 0.5*Max2;
00279         wait (3);
00280         Excecute = true;
00281     }
00282 }
00283 
00284 int main()
00285 {
00286     upperlimit = n1*twopi; // Calculation for limits of the robot
00287     turnlimit = n2*twopi;
00288     ledF = 1;
00289     ledC = 1;
00290     PowerMotor.write(0);
00291     PowerMotor2.write(0);
00292     MotorWrite.attach(MotorSet,1/Fs); // Activate the tickers
00293     biquadTicker.attach(myController,1/Fs2);
00294     PowerServo.period_ms(20);
00295     while (true) {
00296         Calibration(); // Makes it able to calibrate during firing mode
00297         while (Excecute) {
00298             // Motor 1
00299             if (Rotatie >= upperlimit) { // If OutRange
00300                 Goal = upperlimit - margin-0.2;
00301                 OutRange = true;
00302             }
00303             if (Rotatie <= downlimit) { // If OutRange
00304                 Goal = 0 + margin +0.2;
00305                 OutRange = true;
00306             }
00307             if (Rotatie >= margin && Rotatie <= upperlimit - margin) { // If in range / not OutRange
00308                 OutRange = false;
00309             }
00310             if (Rotatie >= downlimit && Rotatie <= upperlimit && OutRange == false) { //EMG aansturing when in range / not OutRange
00311                 if (y8 >= T2 ) {
00312                     Direction = 1;
00313                     v = 0.13;
00314                 }
00315                 if (y8 > T1 && y8 < T2) {
00316                     Direction = 0;
00317                     v = 0.07;
00318                 }
00319                 if (y8 <= T1) {
00320                     Direction = 0;
00321                     v = 0;
00322                 }
00323             }
00324 
00325             // Motor
00326             if (Rotatie2 >= turnlimit) { // If OutRange
00327                 Goal2 = turnlimit-0.1;
00328                 OutRange2 = true;
00329             }
00330             if (Rotatie2 <= -turnlimit) { // If OutRange
00331                 Goal2 = -turnlimit+0.1;
00332                 OutRange2 = true;
00333             }
00334             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit) { // If in range / not OutRange
00335                 OutRange2 = false;
00336             }
00337             if (Rotatie2 >= -turnlimit && Rotatie2 <= turnlimit && OutRange2 == false) { //EMG aansturing when in range / not OutRange
00338                 if (y18 >= T4 ) {
00339                     Direction2 = 1;
00340                     v2 = 0.06;
00341                 }
00342                 if (y18 > T3 && y18 < T4) {
00343                     Direction2 = 0;
00344                     v2 = 0.06;
00345                 }
00346                 if (y18 <= T3) {
00347                     Direction2 = 0;
00348                     v2 = 0;
00349                 }
00350             }
00351             if (Button2 >= 0.8) { // Firing Rubber Band Gun
00352                 wait(0.2);
00353                 ledF = 0;
00354                 PowerServo.write(0.27);
00355                 wait (1);
00356                 PowerServo.write(0.04);
00357                 wait (1);
00358                 PowerServo.write(0.27);
00359                 wait (1);
00360                 PowerServo.write(0.04);
00361                 ledF = 1;
00362                 Fired=Fired+1;
00363                 if (Fired == 3) {
00364                     wait (1);
00365                     Home = true;
00366                     Excecute = false;
00367                 }
00368             }
00369             if (Button3 == 0) {
00370                 Excecute = false;
00371                 Home = true;
00372             }
00373             Calibration();
00374         }
00375 
00376         while (Home) { // Return to base position
00377             OutRange = true; // Activation of PID control part
00378             OutRange2 = true;
00379             Goal = margin;
00380             Goal2 = 0;
00381             if (fabs(Error)<=0.1 && fabs(Error2)<=0.1) { // If error is small enough, then firing mode will be enabled again
00382                 timer.start();
00383             } else {
00384                 timer.stop();
00385                 timer.reset();
00386             }
00387             if (timer.read() >= 3) { // Firing mode is being enabled again and values are reset.
00388                 Errori = 0;
00389                 Errord = 0;
00390                 Errorp = 0;
00391                 Errori2 = 0;
00392                 Errord2 = 0;
00393                 Errorp2 = 0;
00394                 Fired = 0;
00395                 wait (5);
00396                 Excecute = true;
00397                 Home = false;
00398             }
00399         }
00400     }
00401 }