main.cpp

00001 #include "mbed.h"
00002 #include "encoder.h"
00003 #include "BiQuad.h"
00004 #include "HIDScope.h"
00005 #include <complex>
00006 #include <iostream>
00007 
00008 
00009 AnalogIn potmeter1(A3);
00010 AnalogIn    emg0( A0 );
00011 AnalogIn    emg1( A1 );
00012 
00013 HIDScope    scope( 3 );
00014 DigitalOut  led(LED1);
00015 
00016 Ticker sample_timer;
00017 Ticker MyTickerMean;
00018 
00019 //Constants------------------------------------
00020 const int n = 10;
00021 //const int n2 = 10;
00022 float emg0_filtered[n] = {}; 
00023 //float mean_filtered[n2] = {};
00024 int count = 0;
00025 int count2 = 0;
00026 const float controller_TS = 0.1;
00027 float mean = 0.0;
00028 float sum = 0.0;
00029 
00030 
00031 //Constants EMG switch
00032 const float LeftFastmin=0.075;
00033 const float LeftFastmax=0.15;
00034 const float LeftSlowmin=0.2;
00035 const float LeftSlowmax=0.35;
00036 const float RightSlowmin=0.4;
00037 const float RightSlowmax=0.65;
00038 const float RightFastmin=0.7;
00039 const float RightFastmax=1.5;
00040 int SignalNumber = 0;
00041 int numberOfSet = 0;
00042 const int action =50;
00043 
00044 
00045 //Functions---------------------------------------
00046 float read_pot(double potmeter){
00047     float pot_value = potmeter;
00048     return pot_value;
00049 }
00050 /*
00051 float get_sum(float array[], const int n){
00052     float sum_math = 0.0;
00053     for (int m=0 ; m<n ; m++ ){
00054         sum_math = sum_math + array[m];
00055     }
00056     return sum_math;   
00057 }*/
00058 
00059 float get_sum(float array[], const int n){
00060     float sum_math = 0.0;
00061     for (int m=0 ; m<n ; m++ ){
00062         sum_math = sum_math + array[m];
00063     }
00064     return sum_math;   
00065 }
00066  
00067 
00068 
00069 float get_mean_value(){
00070     emg0_filtered[count] = read_pot(potmeter1);
00071     count++;
00072     if (count == n){
00073         count = 0;
00074     }
00075     float mean_math = get_sum(emg0_filtered,n)/n;
00076     return mean_math;
00077 }
00078 
00079 //----------------------------------------------------
00080 
00081 
00082 void get_Signal_number(){
00083     mean = get_mean_value();
00084     //Check first case
00085     if( mean < LeftFastmin ) {
00086         if (count2 <action){
00087             mean = get_mean_value();
00088             if(mean < LeftFastmin){
00089                 count2++;
00090             }
00091             else{
00092                 count2=0;
00093                 SignalNumber=0;
00094             } 
00095         }
00096         else{
00097             SignalNumber = 0;
00098             count2=0;
00099         }
00100     }
00101      //Check second case  
00102      else if(mean <= LeftFastmax and mean > LeftFastmin){
00103         if (count2 <action){
00104             mean = get_mean_value();
00105             if(mean <=LeftFastmax and mean>LeftFastmin){
00106                 count2++;
00107             }
00108             else{
00109                 count2=0;
00110                 SignalNumber=0;
00111             } 
00112         }
00113         else{
00114             SignalNumber = 1;
00115             count2=0;
00116         }
00117     }
00118      else if( mean <=LeftSlowmax and mean>LeftSlowmin) {
00119         if (count2 <action){
00120             mean = get_mean_value();
00121             if(mean <=LeftSlowmax and mean>LeftSlowmin){
00122                 count2++;
00123             }
00124             else{
00125                 count2=0;
00126                 SignalNumber=0;
00127             } 
00128         }
00129         else{
00130             SignalNumber = 2;
00131             count2=0;
00132         }
00133      }
00134      else if( mean <=RightSlowmax and mean>RightSlowmin) {
00135         if (count2 <action){
00136             mean = get_mean_value();
00137             if(mean <=RightSlowmax and mean>RightSlowmin){
00138                 count2++;
00139             }
00140             else{
00141                 count2=0;
00142                 SignalNumber=0;
00143             } 
00144         }
00145         else{
00146             SignalNumber = 3;
00147             count2=0;
00148         }
00149      }
00150      else if( mean <=RightFastmax and mean>RightFastmin ) {
00151         if (count2 <action){
00152             mean = get_mean_value();
00153             if(mean <=RightFastmax and mean>RightFastmin){
00154                 count2++;
00155             }
00156             else{
00157                 count2=0;
00158                 SignalNumber=0;
00159             } 
00160         }
00161         else{
00162             SignalNumber = 4;
00163             count2=0;
00164         }
00165     }
00166     else{
00167         count2=0;
00168         SignalNumber =0;
00169     }
00170 }       
00171         
00172 /*switch(SignalNumber)
00173     {   
00174         case 0: //Standstill
00175          motor1Direction=1;
00176          motor1Speed=0;
00177          printf("Motor 1 is standing still\n");
00178          break;
00179         case 1: //Move Left fast
00180          motor1Direction=1;
00181          motor1Speed=1;
00182          printf("Motor 1 is moving left fast\n");
00183          break;
00184         case 2: //Move Left slow (movement speed is half of that of fast movement)
00185          motor1Direction=1;
00186          motor1Speed=0.5;
00187          printf("Motor 1 is moving left slow\n");
00188          break;
00189         case 3: //Move right slow
00190          motor1Direction=0;
00191          motor1Speed=0.5;
00192          printf("Motor 1 is moving right slow\n");
00193          break;
00194         case 4: //Move right fast
00195          motor1Direction=0;
00196          motor1Speed=1;
00197          printf("Motor 1 is moving right fast\n");
00198          break;
00199         default : //if something is wrong, standstill
00200          motor1Direction=1;
00201          motor1Speed=0;
00202          printf("Motor 1 is standing still, something went wrong\n")
00203         
00204 }*/
00205 
00206 
00207 
00208 
00209 ///-----------------------------------------------------------------------
00210 /** Sample function
00211  * this function samples the emg and sends it to HIDScope
00212  **/
00213  
00214 // Example: 4th order Butterworth LP (w_c = 0.1*f_nyquist)
00215 BiQuad LP1( 0.6389437261127493, 1.2778874522254986, 0.6389437261127493, 1.1429772843080919, 0.4127976201429053 ); //Lowpass filter Biquad
00216 BiQuad HP2( 0.8370879899975344, -1.6741759799950688, 0.8370879899975344, -1.6474576182593796, 0.7008943417307579 ); //Highpass filter Biquad
00217 BiQuad NO3( 0.7063988100714527, -1.1429772843080923, 0.7063988100714527, -1.1429772843080923,  0.41279762014290533); //Notch filter Biquad 
00218 BiQuadChain BiQuad_filter;
00219 float Signal;
00220 float Signal_filtered;
00221 
00222 // Add the biquads to the chain
00223 
00224  
00225 void sample()
00226 {
00227     Signal=(emg0+emg1)/2;
00228     Signal_filtered= BiQuad_filter.step(Signal);
00229     /* Set the sampled emg values in channel 0 (the first channel) and 1 (the second channel) in the 'HIDScope' instance named 'scope' */
00230     scope.set(0, emg0.read() );
00231     scope.set(1, emg1.read() );
00232     scope.set(2, Signal_filtered);
00233     /* Repeat the step above if required for more channels of required (channel 0 up to 5 = 6 channels) 
00234     *  Ensure that enough channels are available (HIDScope scope( 2 ))
00235     *  Finally, send all channels to the PC at once */
00236     scope.send();
00237     /* To indicate that the function is working, the LED is toggled */
00238     led = !led;
00239 }
00240 
00241 
00242   
00243 
00244 
00245 
00246 
00247 
00248 int main()
00249 {
00250     BiQuad_filter.add( &LP1 ).add( &HP2 ).add( &NO3);
00251 
00252     MyTickerMean.attach(&get_Signal_number, controller_TS);
00253     sample_timer.attach(&sample, 0.02);
00254     
00255     while (true) {}
00256         
00257 }
00258