main.cpp

00001 #include "mbed.h"
00002 #include "HIDScope.h"
00003 #include "BiQuad.h"
00004 #include "MODSERIAL.h"
00005 
00006 #include "FastPWM.h"  
00007 #include "QEI.h"
00008 float pwmprocent1 = 15;             // introduce duty cycle variable translational motor
00009 float pwmprocent2 = 15;             // introduce duty cycle for rotational motor (SLOW DOWN TO PREVENT OVERSHOOT?)
00010 int modesw;                 // introduce mode to switch from translational to rotational control
00011 FastPWM motorpwm1(D5);       // define PWM Pin
00012 FastPWM motorpwm2(D6);      // define motor 2 pwm pin
00013 float totalpwm1 = pwmprocent1/100;
00014 float totalpwm2 = pwmprocent2/100;
00015 int upperlimit = 1200; 
00016 int lowerlimit = 3920;
00017 DigitalOut m1direction(D4);     // direction translational motor
00018 DigitalOut m2direction(D7);     // direction
00019 
00020 //Define objects
00021     AnalogIn    emg1_in( A0 ); /* read out the signal */
00022     AnalogIn    emg2_in( A1 );
00023     AnalogIn    emg3_in( A2 );
00024     DigitalIn   max_reader1( SW2 );
00025     DigitalIn   max_reader3( SW3 );
00026 
00027     Ticker      main_timer;
00028     Ticker      max_read1;
00029     Ticker      max_read3;
00030     HIDScope    scope( 5 );
00031     DigitalOut  red(LED_RED);
00032     DigitalOut  blue(LED_BLUE);
00033     DigitalOut  green(LED_GREEN);
00034     MODSERIAL   pc(USBTX, USBRX);
00035 
00036 
00037 // EMG variables
00038     //Right Biceps
00039     double emg1;
00040     double emg1highfilter;
00041     double emg1notchfilter;
00042     double emg1abs;
00043     double emg1lowfilter;
00044     double emg1peak;
00045     double maxpart1;
00046     // Left Biceps
00047     double emg2;
00048     double emg2highfilter;
00049     double emg2notchfilter;
00050     double emg2abs;
00051     double emg2lowfilter;
00052     double emg2peak;
00053     double max1;
00054     double maxpart2;
00055     // Left Lower Arm OR Triceps
00056     double emg3;
00057     double emg3highfilter;
00058     double emg3notchfilter;
00059     double emg3abs;
00060     double emg3lowfilter;
00061     double emg3peak;
00062     double max3;
00063     double maxpart3;
00064 
00065 // BiQuad Filter Settings
00066     // Right Biceps
00067     BiQuad filterhigh1(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01); /* Filter at 10 Hz */
00068     BiQuad filternotch1(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01); /* Filter at 50 Hz */
00069     BiQuad filterpeak1(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01); /* 4dB Gain peak at 11 Hz */
00070     BiQuad filterlow1(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01); /* Filter at 15 Hz */
00071     // Left Biceps
00072     BiQuad filterhigh2(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
00073     BiQuad filternotch2(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
00074     BiQuad filterpeak2(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
00075     BiQuad filterlow2(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
00076     // Left Lower Arm OR Triceps
00077     BiQuad filterhigh3(9.704e-01,-1.9408,9.704e-01,-1.9389,9.427e-01);
00078     BiQuad filternotch3(9.495e-01,-1.8062,9.495e-01,-1.8062,8.992e-01);
00079     BiQuad filterpeak3(1.0033,-1.984,9.852e-01,-1.9838,9.8855e-01);
00080     BiQuad filterlow3(1.368e-03,2.737e-03,1.369e-03,-1.9219,9.274e-01);
00081     //
00082 
00083 // stopfunction
00084 void superstop()
00085 {
00086     int stoptimes;
00087     if (modesw==0)
00088     {
00089          motorpwm1.write(0);
00090          for (int a = 0; a < pwmprocent1; a = a + 1)
00091          {
00092              if (m1direction = 0)
00093              {
00094                  m1direction = 1;
00095              }
00096              else
00097              {
00098                  m1direction = 0;
00099              }
00100          }
00101      else
00102      {
00103          motorpwm2.write(0);
00104          for (int a = 0; a < pwmprocent2; a = a+1)
00105          {
00106              if (m2direction = 0)
00107              {
00108                  m2direction = 1;
00109              }
00110              else
00111              {
00112                  m2direction = 0;
00113              }
00114          }
00115      } 
00116 }
00117 
00118 // Finding max values for correct motor switch if the button is pressed
00119 void get_max1(){
00120     if (max_reader1==0){
00121             green = !green;
00122             red = 1;
00123             blue = 1;
00124             
00125             for(int n=0;n<2000;n++){  /* measure 2000 samples and filter it */
00126             emg1 = emg1_in.read();      /* read out emg */
00127             emg1highfilter = filterhigh1.step(emg1); /* high pass filtered */
00128             emg1notchfilter = filternotch1.step(emg1highfilter); /* notch filtered */
00129             emg1abs = fabs(emg1notchfilter); /* take the absolute value */
00130             emg1lowfilter = filterlow1.step(emg1abs); /* low pass filtered */
00131             emg1peak = filterpeak1.step(emg1lowfilter); /* 4dB gain peak */
00132             
00133             if (max1<emg1peak){
00134                 max1 = emg1peak; /* set the max value at the highest measured value */
00135             }
00136             wait(0.001f); /* measure at 1000Hz */   
00137             }
00138             wait(0.2f);
00139             green = 1;
00140     }
00141     maxpart1 = 0.25*max1; /* set cut off voltage at 25% of max for right biceps */
00142     maxpart2 = 0.15*max1; /* set cut off votage at 15% of max for left biceps */
00143 }
00144 
00145 void get_max3(){
00146     if (max_reader3==0){
00147             green = 1;
00148             blue = 1;
00149             red = !red;
00150             for(int n=0;n<2000;n++){
00151                         
00152             emg3 = emg3_in.read();
00153             emg3highfilter = filterhigh3.step(emg3);
00154             emg3notchfilter = filternotch3.step(emg3highfilter);
00155             emg3abs = fabs(emg3notchfilter);
00156             emg3lowfilter = filterlow3.step(emg3abs);
00157             emg3peak = filterpeak3.step(emg3lowfilter);
00158             
00159             if (max3<emg3peak){
00160                 max3 = emg3peak; /* set the max value at the highest measured value */
00161             }
00162             wait(0.001f);    
00163             }
00164             wait(0.2f);
00165             red = 1;
00166     }
00167     maxpart3 = 0.25*max3; /* set cut off voltage at 25% of max for right biceps */
00168 }
00169 
00170 // Filtering & Scope
00171 void filter() {
00172     // Right Biceps
00173     emg1 = emg1_in.read();
00174     emg1highfilter = filterhigh1.step(emg1);
00175     emg1notchfilter = filternotch1.step(emg1highfilter);
00176     emg1abs = fabs(emg1notchfilter);
00177     emg1lowfilter = filterlow1.step(emg1abs);
00178     emg1peak = filterpeak1.step(emg1lowfilter); /* Final Right Biceps values to be sent */
00179     // Left Biceps
00180     emg2 = emg2_in.read();
00181     emg2highfilter = filterhigh2.step(emg2);
00182     emg2notchfilter = filternotch2.step(emg2highfilter);
00183     emg2abs = fabs(emg2notchfilter);
00184     emg2lowfilter = filterlow2.step(emg2abs);
00185     emg2peak = filterpeak2.step(emg2lowfilter); /* Final Left Biceps values to be sent */
00186     // Left Lower Arm OR Triceps
00187     emg3 = emg3_in.read();
00188     emg3highfilter = filterhigh3.step(emg3);
00189     emg3notchfilter = filternotch3.step(emg3highfilter);
00190     emg3abs = fabs(emg3notchfilter);
00191     emg3lowfilter = filterlow3.step(emg3abs);
00192     emg3peak = filterpeak3.step(emg3lowfilter); /* Final Lower Arm values to be sent */
00193     int count;
00194     QEI Encoder(D12,D13,NC,64);
00195     /* Compare measurement to the calibrated value to decide actions */
00196     if (maxpart1<emg1peak){ /* See if right biceps is contracting */
00197         red = 0;
00198         blue = 1;
00199         green = 1;
00200         
00201         switch (modesw)
00202         {
00203             case 0:
00204             {
00205                 modesw = 1;
00206                 wait(1.0);
00207                 break;
00208             }
00209             case 1:
00210             {
00211                 modesw = 0;
00212                 wait(1.0);
00213                 break;
00214             }
00215 
00216         }
00217     else {
00218         if (maxpart2<emg2peak){ /* See if left biceps is contracting */
00219             red = 1;
00220             blue = 0;
00221             green = 1;
00222    
00223                 switch (modesw)
00224                 {
00225                     case 0:
00226                     {
00227                         m1direction = 1;
00228                         motorpwm1.write(totalpwm1);
00229                         break;
00230                     }
00231                     case 1:
00232                     {
00233                         count = Encoder.getPulses();
00234                         m2direction = 1;
00235                         if (count<lowerlimit)
00236                         {
00237                             motorpwm2.write(totalpwm2);
00238                         break;
00239                         }
00240                         else
00241                         {
00242                             superstop();
00243                     }
00244                 }
00245                 
00246             }
00247         
00248     else {
00249         if (maxpart3<emg3peak){ /* See if lower arm is contracting */
00250             red = 1;
00251             blue = 1;
00252             green = 0;
00253                         
00254             switch (modesw)
00255             {
00256                 case 0:
00257                 {
00258                     m1direction = 0;
00259                     motorpwm1.write(totalpwm1);
00260                     break;
00261                 }
00262                 case 1:
00263                 {             
00264                     If (count>higherlimit)
00265                     {
00266                         m1direction = 0;
00267                         motorpwm2.write(totalpwm1);
00268                         break;
00269                     }
00270                     else
00271                     superstop();
00272                 }
00273             }
00274         
00275     else {
00276         red = 1; /* Shut down all led colors if no movement is registered */
00277         blue = 1;
00278         green = 1;
00279         }
00280         }
00281         }
00282     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
00283     scope.set(0, emg1peak ); /* plot Right biceps voltage */
00284     scope.set(1, emg2peak ); /* Plot Left biceps voltage */
00285     scope.set(2, maxpart1 ); /* Show the line above which the motor should run for right biceps */
00286     scope.set(3, emg3peak ); /* Plot Lower Arm voltage */
00287     scope.set(4, maxpart3 ); /* Plot the line above which the motor should run for lower arm */
00288  
00289     scope.send(); /* send everything to the HID scope */
00290 }
00291 
00292 int main(){   
00293 
00294     main_timer.attach(&filter, 0.001); /* set frequency for the filters at 1000Hz */
00295     max_read1.attach(&get_max1, 2); /* set the frequency of the calibration loop at 0.5Hz */
00296     max_read3.attach(&get_max3, 2);
00297     while(1) {}
00298 }