main.cpp

00001 //libaries
00002 #include "mbed.h"
00003 #include "BiQuad.h"
00004 #include "HIDScope.h"
00005 #include "encoder.h"
00006 #include "MODSERIAL.h"
00007 
00008 // Variables
00009 //State Machine and calibration
00010 enum States {CalEMG, SelectDevice, EMG, Rest, Demonstration};
00011 States State;
00012 DigitalIn button (D1);
00013 
00014 //Buttons  en leds voor calibration
00015 DigitalIn button1(PTA4);
00016 DigitalOut led(D2);
00017 
00018 //MVC for calibration
00019 double MVCLB = 0; double MVCRB = 0; double MVCLT = 0; double MVCRT = 0;
00020 //MEAN for calibration - rest
00021 double RESTMEANLB = 0; double RESTMEANRB =0; double RESTMEANLT = 0; double RESTMEANRT = 0;
00022 double emgMEANSUBLB; double emgMEANSUBRB; double emgMEANSUBLT; double emgMEANSUBRT;
00023 double emgSUMLB; double emgSUMRB; double emgSUMLT; double emgSUMRT;
00024 
00025 bool caldone = false; 
00026 int CalibrationSample = 1000; //How long will we calibrate? Timersampletime*Calibrationsample
00027 
00028 int Timescalibration = 0;
00029 int TimescalibrationREST = 0;
00030 
00031 //Encoder and motor
00032 double Huidigepositie1;
00033 double Huidigepositie2;
00034 double motorValue1;
00035 double motorValue2;
00036 Ticker Treecko;             //We make an awesome ticker for our control system
00037 PwmOut M1E(D6);             //Biorobotics Motor 1 PWM control of the speed 
00038 PwmOut M2E(D5);
00039 DigitalOut M1D(D7);         //Biorobotics Motor 1 diraction control
00040 Encoder motor1(D13,D12,true);
00041 Encoder motor2(D9,D8,true);
00042 DigitalOut M2D(D4);
00043 double PwmPeriod = 1.0/5000.0;
00044 
00045 //Demonstration
00046 AnalogIn potMeter2(A1);
00047 AnalogIn potMeter1(A2);
00048 
00049 MODSERIAL pc(USBTX,USBRX);
00050 
00051 //PID
00052 const double Ts = 0.002f;                   // tickertime/ sample time
00053 double e_prev = 0; 
00054 double e_int = 0;
00055 double e_prev2 = 0;
00056 double e_int2 = 0;
00057 
00058 // EMG and Filters
00059 // Biquad filters voor Left Biceps (LB): Notch, High-pass and Low-pass filter
00060 BiQuad N1LB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
00061 BiQuadChain NFLB;
00062 BiQuad HP1LB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
00063 BiQuad HP2LB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
00064 BiQuadChain HPFLB;
00065 BiQuad LP1LB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
00066 BiQuad LP2LB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
00067 BiQuadChain LPFLB;
00068 
00069 // Biquad filters voor Right Biceps (RB): Notch, High-pass and Low-pass filter
00070 BiQuad N1RB( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
00071 BiQuadChain NFRB;
00072 BiQuad HP1RB( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
00073 BiQuad HP2RB( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
00074 BiQuadChain HPFRB;
00075 BiQuad LP1RB( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
00076 BiQuad LP2RB( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
00077 BiQuadChain LPFRB;
00078 
00079 // Biquad filters voor Left Triceps (LT): Notch, High-pass and Low-pass filter
00080 BiQuad N1LT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
00081 BiQuadChain NFLT;
00082 BiQuad HP1LT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
00083 BiQuad HP2LT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
00084 BiQuadChain HPFLT;
00085 BiQuad LP1LT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
00086 BiQuad LP2LT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
00087 BiQuadChain LPFLT;
00088 
00089 // Biquad filters for Right Triceps (RT): Notch, High-pass and Low-pass filter
00090 BiQuad N1RT( 8.63271e-01, -1.39680e+00, 8.63271e-01, -1.39680e+00, 7.26543e-01 );
00091 BiQuadChain NFRT;
00092 BiQuad HP1RT( 9.63001e-01, -9.62990e-01, 0.00000e+00, -9.62994e-01, 0.00000e+00 );
00093 BiQuad HP2RT( 1.00000e+00, -2.00001e+00, 1.00001e+00, -1.96161e+00, 9.63007e-01 ); 
00094 BiQuadChain HPFRT;
00095 BiQuad LP1RT( 2.56971e-06, 2.56968e-06, 0.00000e+00, -9.72729e-01, 0.00000e+00 );
00096 BiQuad LP2RT( 1.00000e+00, 2.00001e+00, 1.00001e+00, -1.97198e+00, 9.72734e-01 );
00097 BiQuadChain LPFRT;
00098 
00099 double emgNotchLB;
00100 double emgHPLB;
00101 double emgAbsHPLB;
00102 double emgLPLB;
00103 
00104 double emgNotchRB;
00105 double emgHPRB;
00106 double emgAbsHPRB;
00107 double emgLPRB;
00108 
00109 double emgNotchLT;
00110 double emgHPLT;
00111 double emgAbsHPLT;
00112 double emgLPLT;
00113 
00114 double emgNotchRT;
00115 double emgHPRT;
00116 double emgAbsHPRT;
00117 double emgLPRT;
00118 
00119 AnalogIn emgLB(A0);   // read EMG
00120 AnalogIn emgRB(A1);
00121 AnalogIn emgLT(A2);
00122 AnalogIn emgRT(A3);
00123 
00124 volatile double LBF;
00125 volatile double RBF;
00126 volatile double LTF;
00127 volatile double RTF;
00128 
00129 // RKI
00130 double pi = 3.14159265359;
00131 double q1 = (pi/2);                     //Reference position angle 1 in radiance
00132 double q2 = -(pi/2);                    //Reference position angle 2 in radiance
00133 const double L1 = 0.30;                 //Length arm 1 in mm
00134 const double L2 = 0.38;                 //Length arm 2 in mm
00135 double B1 = 1;                          //Friction constant motor 1
00136 double B2 = 1;                          //Friction constant motor 2
00137 double K = 1;                           //Spring constant movement from end-effector position to setpoint position
00138 double Tijd = 1;                        //Timestep value
00139 double Rsx = 0.38;                      //Reference x-component of the setpoint radius
00140 double Rsy = 0.30;                      //Reference y-component of the setpoint radius
00141 double refP = 0;                        //Reference position motor 1
00142 double refP2 = 0;                       //Reference position motor 2
00143 double Rex = cos(q1)*L1 - sin(q2)*L2;   //The x-component of the end-effector radius 
00144 double Rey = sin(q1)*L1 + cos(q2)*L2;   //The y-component of the end-effector radius
00145 double R1x = 0;                         //The x-component of the joint 1 radius
00146 double R1y = 0;                         //The y-component of the joint 1 radius
00147 double R2x = cos(q1)*L1;                //The x-component of the joint 2 radius
00148 double R2y = sin(q1)*L1;                //The y-component of the joint 2 radius   
00149 double Fx = 0;
00150 double Fy = 0;
00151 double Tor1 = 0;
00152 double Tor2 = 0;
00153 double w1= 0;
00154 double w2= 0;
00155 
00156 // Functions
00157 void Filteren() 
00158 {  
00159     emgNotchLB = NFLB.step(emgLB.read() );    // Notch filter
00160     emgHPLB = HPFLB.step(emgNotchLB);         // High-pass filter: also normalises around 0.
00161     emgAbsHPLB = abs(emgHPLB);                // Take absolute value
00162     emgLPLB = LPFLB.step(emgAbsHPLB);         // Low-pass filter: creates envelope
00163     emgMEANSUBLB = emgLPLB - RESTMEANLB;      // Substract the restmean value
00164     LBF = emgLPLB/MVCLB;                      // Scale to maximum signal: useful for motor. LBF should now be between 0-1.
00165     
00166     emgNotchRB = NFRB.step(emgRB.read()); 
00167     emgHPRB = HPFRB.step(emgNotchRB); 
00168     emgAbsHPRB = abs(emgHPRB);
00169     emgLPRB = LPFRB.step(emgAbsHPRB);
00170     emgMEANSUBLB = emgLPLB - RESTMEANLB;
00171     RBF = emgLPRB/MVCRB;
00172     
00173     emgNotchLT = NFLT.step(emgLT.read() );
00174     emgHPLT = HPFLT.step(emgNotchLT);
00175     emgAbsHPLT = abs(emgHPLT);
00176     emgLPLT = LPFLT.step(emgAbsHPLT);
00177     emgMEANSUBLT = emgLPLT - RESTMEANLT;
00178     LTF = emgLPLT/MVCLT;
00179     
00180     emgNotchRT = NFRT.step(emgRT.read() );
00181     emgHPRT = HPFRT.step(emgNotchRT);
00182     emgAbsHPRT = abs(emgHPRT);
00183     emgLPRT = LPFRT.step(emgAbsHPRT);
00184     emgMEANSUBRT = emgLPRT - RESTMEANRT;
00185     RTF = emgLPRT/MVCRT;
00186 }
00187 
00188 void CalibrationEMG()
00189 {
00190     pc.printf("Timescalibration = %i \r\n",Timescalibration);
00191     Timescalibration++;
00192     
00193     if(Timescalibration<2000)
00194     {
00195         pc.printf("calibration rest EMG, 12 seconds \r\n");
00196         led = 1;
00197         
00198         emgNotchLB = NFLB.step(emgLB.read() );
00199         emgHPLB = HPFLB.step(emgNotchLB);   
00200         emgAbsHPLB = abs(emgHPLB);
00201         emgLPLB = LPFLB.step(emgAbsHPLB);
00202         emgSUMLB += emgLPLB;                         //SUM all rest values LB
00203     
00204         emgNotchRB = NFRB.step(emgRB.read()); 
00205         emgHPRB = HPFRB.step(emgNotchRB);
00206         emgAbsHPRB = abs(emgHPRB);
00207         emgLPRB = LPFRB.step(emgAbsHPRB);
00208         emgSUMRB += emgLPRB;                        //SUM all rest values RB
00209     
00210         emgNotchLT = NFLT.step(emgLT.read() );
00211         emgHPLT = HPFLT.step(emgNotchLT);
00212         emgAbsHPLT = abs(emgHPLT);
00213         emgLPLT = LPFLT.step(emgAbsHPLT);
00214         emgSUMLT += emgLPLT;                         //SUM all rest values LT
00215     
00216         emgNotchRT = NFRT.step(emgRT.read() );  
00217         emgHPRT = HPFRT.step(emgNotchRT);       
00218         emgAbsHPRT = abs(emgHPRT);           
00219         emgLPRT = LPFRT.step(emgAbsHPRT);
00220         emgSUMRT += emgLPRT;                         //SUM all rest values RT
00221     }
00222     if(Timescalibration==1999)
00223     {
00224         led = 0;
00225         RESTMEANLB = emgSUMLB/Timescalibration; //determine the mean rest value
00226         RESTMEANRB = emgSUMRB/Timescalibration; //determine the mean rest value
00227         RESTMEANRT = emgSUMRT/Timescalibration; //determine the mean rest value
00228         RESTMEANLT = emgSUMLT/Timescalibration; //determine the mean rest value
00229     }
00230     if(Timescalibration>2000 && Timescalibration<3000)
00231     {
00232         pc.printf("maximum left biceps, 6 seconds \r\n");
00233         led = 1;
00234         emgNotchLB = NFLB.step(emgLB.read() ); 
00235         emgHPLB = HPFLB.step(emgNotchLB);        
00236         emgAbsHPLB = abs(emgHPLB);          
00237         emgLPLB = LPFLB.step(emgAbsHPLB);       
00238         double emgfinalLB = emgLPLB;
00239         if (emgfinalLB > MVCLB)
00240         {                                       //determine what the highest reachable emg signal is
00241             MVCLB = emgfinalLB;
00242         }    
00243     }
00244     
00245     if(Timescalibration>3000 && Timescalibration<4000)
00246     {
00247         pc.printf(" maximum right biceps, 6 seconds \r\n");
00248         led = 0;
00249         emgNotchRB = NFRB.step(emgRB.read()); 
00250         emgHPRB = HPFRB.step(emgNotchRB); 
00251         emgAbsHPRB = abs(emgHPRB);           
00252         emgLPRB = LPFRB.step(emgAbsHPRB);      
00253         double emgfinalRB = emgLPRB;
00254         if (emgfinalRB > MVCRB)
00255         {                                       //determine what the highest reachable emg signal is
00256             MVCRB = emgfinalRB;
00257         }    
00258     }
00259     
00260     if(Timescalibration>4000 && Timescalibration<5000)
00261     {
00262         pc.printf("maximum left triceps, 6 seconds \r\n");
00263         led = 1;
00264         emgNotchLT = NFLT.step(emgLT.read() );
00265         emgHPLT = HPFLT.step(emgNotchLT);
00266         emgAbsHPLT = abs(emgHPLT);
00267         emgLPLT = LPFLT.step(emgAbsHPLT);
00268         double emgfinalLT = emgLPLT;
00269         if (emgfinalLT > MVCLT)
00270         {                                       //determine what the highest reachable emg signal is
00271             MVCLT = emgfinalLT;
00272         }    
00273     }
00274     
00275     if(Timescalibration>5000 && Timescalibration<6000)
00276     {
00277         pc.printf("maximum right triceps, 6 seconds \r\n");
00278         emgNotchRT = NFRT.step(emgRT.read() );  
00279         emgHPRT = HPFRT.step(emgNotchRT);       
00280         emgAbsHPRT = abs(emgHPRT);           
00281         emgLPRT = LPFRT.step(emgAbsHPRT);
00282         double emgfinalRT = emgLPRT;
00283         if (emgfinalRT > MVCRT)
00284         {                                       //determine what the highest reachable emg signal is
00285             MVCRT = emgfinalRT;
00286         }    
00287     }
00288     
00289     if(Timescalibration>6000)
00290     {
00291          pc.printf("calibration finished");
00292          State = SelectDevice; 
00293     }
00294 }
00295 
00296 void RKI()
00297 {
00298     Rex = cos(q1)*L1 - sin(q2)*L2;
00299     Rey = sin(q1)*L1 + cos(q2)*L2;
00300     R2x = cos(q1)*L1;
00301     R2y = sin(q1)*L1;
00302     Fx = (Rsx-Rex)*K;
00303     Fy = (Rsy-Rey)*K;
00304     Tor1 = (Rex-R1x)*Fy + (R1y-Rey)*Fx;
00305     Tor2 = (Rex-R2x)*Fy + (R2y-Rey)*Fx;
00306     w1 = Tor1/B1;
00307     w2 = Tor2/B2;
00308     q1 = q1 + w1*Tijd;
00309     q2 = q2 + w2*Tijd;
00310     
00311     refP = (((0.5*pi) - q1)/(2*pi));
00312     refP2 = (( q1 + q2)/(2*pi));    //Get reference positions
00313 }
00314 
00315 void SetpointRobot()                    // Get position from Potmeters
00316 {    
00317     double Potmeterwaarde2 = potMeter2.read();
00318     double Potmeterwaarde1 = potMeter1.read();
00319     
00320     if  (Potmeterwaarde2>0.6) {
00321         Rsx += 0.001;                    //increases 1 mm when potmetervalue above 0.6
00322     }
00323     else if  (Potmeterwaarde2<0.4) {
00324         Rsx -= 0.001;                    //decreases 1 mm when potmetervalue below 0.4
00325     }
00326     else {                               //x value of setpoint doesn't change
00327     }
00328 
00329     if (Potmeterwaarde1>0.6) {           //increases 1 mm when potmetervalue above 0.6
00330         Rsy += 0.001;
00331     }
00332     else if (Potmeterwaarde1<0.4) {      //decreases 1 mm when potmetervalue below 0.4
00333         Rsy -= 0.001;                       
00334     }
00335     else {                               //y value of setpoint doesn't change
00336     }
00337 }
00338 
00339 void changePosition ()  // Get position from EMG signals
00340 {
00341     if (RBF>0.5) {
00342         Rsx +=0.001;    // increases 1 mm
00343     }
00344     else {}
00345     if (RTF>0.5) {
00346         Rsx -=0.001;
00347     }
00348     else {}
00349     if (LBF>0.5) {
00350         Rsy +=0.001;
00351     }
00352     else {}
00353     if (LTF>0.5) {
00354         Rsy -=0.001;
00355     }
00356     else {}
00357 }
00358 
00359 double FeedBackControl(double error, double &e_prev, double &e_int)   // PID controller motor 1
00360 // The values are not correctly tuned.
00361 {
00362     double kp = 3.36;                           // kind of scaled.
00363     double Proportional= kp*error;
00364     
00365     double kd = 3.36;                           // kind of scaled. 
00366     double VelocityError = (error - e_prev)/Ts; 
00367     double Derivative = kd*VelocityError;
00368     e_prev = error;
00369     
00370     double ki = 4.2;                            // kind of scaled.
00371     e_int = e_int+Ts*error;
00372     double Integrator = ki*e_int;
00373     
00374     double motorValue = (Proportional + Integrator + Derivative)/4200;
00375     return motorValue;
00376 }
00377 
00378 double FeedBackControl2(double error2, double &e_prev2, double &e_int2)   // PID controller motor 2
00379 // The values are not correctly tuned.
00380 {
00381     double kp2 = 3.36                           // kind of scaled.
00382     double Proportional2= kp2*error2;
00383     
00384     double kd2 = 3.36;                          // kind of scaled. 
00385     double VelocityError2 = (error2 - e_prev2)/Ts; 
00386     double Derivative2 = kd2*VelocityError2;
00387     e_prev2 = error2;
00388     
00389     double ki2 = 2.1;                           // kind of scaled.
00390     e_int2 = e_int2+Ts*error2;
00391     double Integrator2 = ki2*e_int2;
00392     
00393     double motorValue2 = Proportional2 + Integrator2 + Derivative2;
00394     return motorValue2;
00395 }
00396 
00397 void SetMotor1(double motorValue)
00398 {
00399     if (motorValue >= 0) {
00400         M1D = 0;
00401     }
00402     else {
00403         M1D = 1;
00404     }
00405     if  (fabs(motorValue) > 1) {
00406         M1E = 1;                    //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
00407     }
00408     else {    
00409         M1E = fabs(motorValue);      //absolute velocity determined, motor is "off" at value of 0
00410     }
00411 }
00412 
00413 void SetMotor2(double motorValue2)
00414 {
00415     if (motorValue2 >= 0) {
00416         M2D = 1;
00417     }
00418     else {
00419         M2D = 0;
00420     }
00421     if  (fabs(motorValue2) > 1) {
00422         M2E = 1;                    //velocity downscaled to 8.4 rad/s (= maximum velocity, value = 1)
00423     }
00424     else {    
00425         M2E = fabs(motorValue2);      //absolute velocity determined, motor is "off" at value of 0
00426     }
00427 }
00428 
00429 void MeasureAndControl(void)
00430 {
00431     RKI();
00432     // control of 1st motor
00433     double Huidigepositie = motor1.getPosition()/4200; 
00434     double error = (refP - Huidigepositie);// make an error
00435     double motorValue = FeedBackControl(error, e_prev, e_int);
00436     SetMotor1(motorValue);
00437     // control of 2nd motor
00438     double Huidigepositie2 = motor2.getPosition()/4200; 
00439     double error2 = (refP2 - Huidigepositie2);// make an error
00440     double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
00441     SetMotor2(motorValue2);
00442 }
00443 
00444 void Loop_funtion()
00445 {   
00446     pc.printf("state machine begint \r\n");
00447     switch(State)
00448     {    
00449         case CalEMG: // Calibration EMG
00450             CalibrationEMG();   //calculates average EMGFiltered at rest and measures max signal EMGFiltered.
00451             break;    
00452         case SelectDevice:      //Looks at the difference between current position and home. Select aansturen EMG or buttons
00453             State = EMG;
00454             if (button==1) {
00455                 State=EMG;
00456             }
00457             else {           // button==0
00458                 State=Demonstration;
00459             }
00460             break;
00461         case EMG: //Control by EMG
00462             Filteren();
00463             changePosition();
00464             MeasureAndControl();
00465             if (button==0) {
00466                 State=Rest;
00467             }
00468             else {}
00469             break;
00470         case Rest: // When it is not your turn, the robot shouldn't react on muscle contractions.
00471             refP=refP;
00472             refP2=refP2;
00473             double Huidigepositie = motor1.getPosition()/4200; 
00474             double error = (refP - Huidigepositie);// make an error
00475             double motorValue = FeedBackControl(error, e_prev, e_int);
00476             SetMotor1(motorValue);
00477             // control of 2nd motor
00478             double Huidigepositie2 = motor2.getPosition()/4200; 
00479             double error2 = (refP2 - Huidigepositie2);// make an error
00480             double motorValue2 = FeedBackControl2(error2, e_prev2, e_int2);
00481             SetMotor2(motorValue2);
00482             
00483             if ( button==1) {
00484                 State=EMG;
00485             }
00486             else {}
00487             break;
00488         case Demonstration: // Control with Potmeters
00489             SetpointRobot();
00490             MeasureAndControl();
00491             break;
00492         }
00493 } 
00494 
00495 int main()
00496 {
00497     //for filtering EMG
00498     //Left Biceps
00499     NFLB.add( &N1LB );
00500     HPFLB.add( &HP1LB ).add( &HP2LB );
00501     LPFLB.add( &LP1LB ).add( &LP2LB );
00502     
00503     //Right Biceps
00504     NFRB.add( &N1RB );
00505     HPFRB.add( &HP1RB ).add( &HP2RB );
00506     LPFRB.add( &LP1RB ).add( &LP2RB );  
00507     
00508     //Left Triceps
00509     NFLT.add( &N1LT );
00510     HPFLT.add( &HP1LT ).add( &HP2LT );
00511     LPFLT.add( &LP1LT ).add( &LP2LT ); 
00512     
00513     //Right Triceps
00514     NFRT.add( &N1RT );
00515     HPFRT.add( &HP1RT ).add( &HP2RT );
00516     LPFRT.add( &LP1RT ).add( &LP2RT ); 
00517     
00518     // serial
00519     pc.baud(115200);
00520     
00521     //motor
00522     M1E.period(PwmPeriod); //set PWMposition at 5000hz
00523    
00524     //State Machine
00525     State = CalEMG;
00526     Treecko.attach(&Loop_funtion, Ts);
00527     while(true)
00528     {}
00529 
00530  }