main.cpp

00001  //--------------------------------------------------------------------------------------------------------------------------//
00002  // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
00003  //--------------------------------------------------------------------------------------------------------------------------//
00004  // Libraries
00005  //--------------------------------------------------------------------------------------------------------------------------//
00006 #include "mbed.h"
00007 #include "MODSERIAL.h"
00008 #include "HIDScope.h"
00009 #include "QEI.h"
00010 #include "biquadFilter.h"
00011 
00012 //--------------------------------------------------------------------------------------------------------------------------//
00013 // Constanten/Inputs/Outputs
00014 //--------------------------------------------------------------------------------------------------------------------------//
00015     MODSERIAL   pc(USBTX, USBRX);               // To/From PC
00016     QEI         Encoder2(D3, D2, NC, 32);       // Encoder Motor 2
00017     QEI         Encoder1(D13,D12,NC, 32);       // Encoder Motor 1
00018     HIDScope    scope(4);                       // Scope, 4 channels
00019     AnalogIn    EMG_left(A0);                   // EMG Analog input LEFT
00020     AnalogIn    EMG_right(A1);                  // EMG Analog input RIGHT
00021 
00022 
00023 // LEDs
00024     DigitalOut LedR(LED_RED);
00025     DigitalOut LedG(LED_GREEN);
00026     DigitalOut LedB(LED_BLUE);
00027 
00028 // Motor
00029     DigitalOut  motor1direction(D7);         // Motor 1, Direction & Speed
00030     PwmOut      motor1speed(D6);
00031     DigitalOut  motor2direction(D4);         // Motor 2, Direction & Speed
00032     PwmOut      motor2speed(D5);
00033 
00034 // Tickers
00035     Ticker ScopeTime;
00036     Ticker myControllerTicker2;
00037     Ticker myControllerTicker1;
00038     // EMG Tickers
00039     Ticker SampleEMGLeft;
00040     Ticker SampleEMGRight;
00041     Ticker ScopeTimer;
00042     Ticker serial;
00043     Ticker MovingAverageLeft;
00044     Ticker MovingAverageRight;
00045 
00046 // Constants
00047     double reference2, reference1;
00048     double position2 = 0, position1 = 0;
00049     double m2_ref = 0, m1_ref = 0;
00050     int count = 0;
00051     double Grens2 = 90, Grens1 = 90;
00052     double Stapgrootte = 5;
00053 
00054 //Sample time (motor-step)
00055     const double m2_Ts = 0.01, m1_Ts = 0.01;
00056 
00057 //Controller gain Motor 2 & 1
00058     const double m2_Kp = 0.5, m2_Ki = 0.005, m2_Kd = 0.5;
00059     const double m1_Kp = 0.5,m1_Ki = 0.005, m1_Kd = 0.5;
00060     double m2_err_int = 0, m2_prev_err = 0;
00061     double m1_err_int = 0, m1_prev_err = 0;
00062 
00063 // Declaring variables EMG
00064     double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
00065     double EMG_L_fh=0;
00066     double EMG_left_value;
00067     double EMG_left_f1;
00068     double EMG_left_f2;
00069     double EMG_left_f3;
00070     double EMG_left_abs;
00071 
00072     double EMG_right_value;
00073     double EMG_right_f1;
00074     double EMG_right_f2;
00075     double EMG_right_f3;
00076     double EMG_right_abs;
00077     double Threshold1;
00078     double Threshold2;
00079     double Threshold3;
00080     double Threshold4;
00081     
00082     int N = 50;
00083     double MAF_left[50];
00084     double EMG_left_MAF;
00085     double MAF_right[50];
00086     double EMG_right_MAF;
00087     
00088 // Filtercoëfficiënten EMG
00089     const double BiGainEMG_H1 = 0.796821; 
00090     const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
00091     
00092     const double BiGainEMG_L1= 0.001041;
00093     const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
00094    
00095     const double BiGainEMG_N1 = 1.0;
00096     const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
00097 
00098 //--------------------------------------------------------------------------------------------------------------------------//
00099 // General Functions
00100 //--------------------------------------------------------------------------------------------------------------------------//
00101 
00102 //HIDScope
00103     void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
00104     {
00105         scope.set(0, reference2 - position2);
00106         scope.set(1, position2);
00107         scope.set(2, EMG_left_MAF);    
00108         scope.set(3, EMG_right_MAF);
00109         scope.send();
00110     
00111     }
00112 
00113 // Reusable PID controller
00114     double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
00115     {
00116     // Derivative
00117         double e_der = (e-e_prev)/Ts;
00118         e_prev = e;
00119     // Integral
00120         e_int = e_int + Ts*e;
00121     // PID
00122         return Kp * e + Ki*e_int + Kd*e_der;
00123     }
00124 //--------------------------------------------------------------------------------------------------------------------------//
00125 // EMG filtering function
00126 //--------------------------------------------------------------------------------------------------------------------------//
00127 
00128 // Filter creation    
00129     biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
00130     biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter 
00131     biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
00132     
00133     biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
00134     biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter  
00135     biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
00136 
00137 void EMGfilterLeft()
00138 {
00139     EMG_left_value = EMG_left.read();
00140     EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
00141     EMG_left_abs = fabs(EMG_left_f1);
00142     EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs);
00143     EMG_left_f3 = EMG_notch1.step(EMG_left_f2);
00144 
00145 }
00146 
00147 void EMGfilterRight()
00148 {
00149     EMG_right_value = EMG_right.read();
00150     EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
00151     EMG_right_abs = fabs(EMG_right_f1);
00152     EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs);
00153     EMG_right_f3 = EMG_notch1R.step(EMG_right_f2);
00154 }
00155 
00156 // Movingaverage Filter
00157     void MovingAverageFilterLeft()
00158     {
00159         EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N;
00160         MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25];
00161         MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0];
00162         MAF_left[0] = EMG_left_f3;
00163     }
00164     
00165     void MovingAverageFilterRight()
00166     {
00167         EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N;
00168         MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25];
00169         MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0];
00170         MAF_right[0] = EMG_right_f3;
00171     }
00172 
00173 //--------------------------------------------------------------------------------------------------------------------------//
00174 // Motor control functions
00175 //--------------------------------------------------------------------------------------------------------------------------//
00176 
00177 // Motor2 control
00178     void motor2_Controller() 
00179     {
00180         // Setpoint motor 2
00181             reference2 = m2_ref;                           // Reference in degrees
00182             position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
00183         // Speed control
00184             double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
00185             double m2_P2 = m2_P1;
00186             motor2speed = abs(m2_P2); 
00187         // Direction control
00188             if(m2_P2 > 0) 
00189             {    
00190                 motor2direction = 0;
00191             }
00192             else
00193             {
00194                 motor2direction = 1;
00195             }
00196     }   
00197 
00198 // Motor1 control
00199     void motor1_Controller() 
00200     {
00201         // Setpoint Motor 1
00202             reference1 = m1_ref;                           // Reference in degrees
00203             position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
00204         // Speed control
00205             double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err); 
00206             double m1_P2 = m1_P1;
00207             motor1speed = abs(m1_P2); 
00208         // Direction control    
00209             if(m1_P2 > 0)
00210             {  
00211                 motor1direction = 1;
00212             }
00213             else
00214             {
00215                 motor1direction = 0;
00216             }
00217     }
00218 
00219 //--------------------------------------------------------------------------------------------------------------------------//
00220 // Main function
00221 //--------------------------------------------------------------------------------------------------------------------------//
00222 int main()
00223 {  
00224 //--------------------------------------------------------------------------------------------------------------------------//
00225 // Initalizing
00226 //--------------------------------------------------------------------------------------------------------------------------// 
00227     //LEDs OFF
00228         LedR = LedB = LedG = 1;
00229     
00230     //PC connection & check
00231         pc.baud(115200);
00232         pc.printf("Tot aan loop werkt\n");
00233     
00234     // Tickers
00235         ScopeTime.attach(&ScopeSend, 0.01f);                        // 100 Hz, Scope
00236         myControllerTicker2.attach(&motor2_Controller, 0.01f );     // 100 Hz, Motor 2
00237         myControllerTicker1.attach(&motor1_Controller, 0.01f );     // 100 Hz, Motor 1
00238         SampleEMGLeft.attach(&EMGfilterLeft, 0.002);                // 500 Hz, EMG LEFT
00239         SampleEMGRight.attach(&EMGfilterRight, 0.002);              // 500 Hz, EMG RIGHT
00240         MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.005);  // 200 Hz, MAF LEFT
00241         MovingAverageRight.attach(&MovingAverageFilterRight, 0.005);// 200 Hz, MAF RIGHT
00242         
00243 //--------------------------------------------------------------------------------------------------------------------------//
00244 // Determing Threshold 
00245 //--------------------------------------------------------------------------------------------------------------------------//      
00246         wait(20);
00247     
00248         LedG.write(1);
00249         wait(0.2);
00250         LedG.write(0);
00251         wait(0.2);
00252         LedG.write(1);
00253         wait(0.2);
00254         LedG.write(0);
00255         wait(0.2);
00256         LedG.write(1);
00257         wait(0.2);    
00258         LedG.write(0);
00259         wait(2);
00260         Threshold1 = 0.5*EMG_left_MAF;
00261         Threshold2 = 0.2*EMG_left_MAF;
00262         LedG.write(1);
00263         LedR.write(0);
00264         wait(2);
00265         LedR.write(1);
00266 
00267         wait(2);
00268         LedB.write(1);
00269         wait(0.2);
00270         LedB.write(0);
00271         wait(0.2);
00272         LedB.write(1);
00273         wait(0.2);
00274         LedB.write(0);
00275         wait(0.2);
00276         LedB.write(1);
00277         wait(0.2);    
00278         LedB.write(0);
00279         wait(2);
00280         Threshold3 = 0.5*EMG_right_MAF;
00281         Threshold4 = 0.2*EMG_right_MAF;
00282         LedB.write(1);
00283         pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4); 
00284         LedR.write(0);
00285         wait(2);
00286         LedR.write(1);
00287     
00288     
00289 //--------------------------------------------------------------------------------------------------------------------------//
00290 // Control Program
00291 //--------------------------------------------------------------------------------------------------------------------------//
00292     while(true)
00293     {
00294     // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
00295         if(true)
00296         {
00297                 
00298                 LedR = LedB = 1;
00299                 LedG = 1;
00300                 if(EMG_left_MAF > Threshold1)
00301                 {
00302                     m2_ref = m2_ref + Stapgrootte;
00303                     m1_ref = m1_ref - Stapgrootte;
00304                     wait(0.1);
00305                     LedG = 0;
00306                     if (m2_ref > Grens2)
00307                     {
00308                         m2_ref = Grens2;
00309                         m1_ref = -1*Grens1;
00310                     }
00311                 }
00312                 if(EMG_right_MAF > Threshold3)
00313                 {
00314                     m2_ref = m2_ref - Stapgrootte;
00315                     m1_ref = m1_ref + Stapgrootte;
00316                     wait(0.1);
00317                     LedB = 0;
00318                     if (m2_ref < -1*Grens2)
00319                     {
00320                         m2_ref = -1*Grens2;
00321                         m1_ref = Grens1;
00322                     }
00323                 }
00324         }
00325     }
00326 }