File content as of revision 8:237b1e262ebd:

#include "mbed.h"
#include "HIDScope.h"                      //require the HIDScope library
#include "MODSERIAL.h"
#include "BiQuad.h"
# include "math.h"
//                       Rechterarm
//                       DEFINING                  
//Define Inputs
//rechterarm
AnalogIn emg_r(A0);                               //analog of emg_r input
//linkerarm
AnalogIn emg_l(A1);

InterruptIn button1(PTA4);                        //test button for starting motor 1
InterruptIn button2(SW2);                         //FOR DEBUGGING

//Define Outputs
DigitalOut led1(LED_RED);
DigitalOut led2(LED_BLUE);
DigitalOut led3(LED_GREEN);                     //FOR DEBUGGING
MODSERIAL pc(USBTX,USBRX);


//Define Tickers
//rechterarm
Ticker      LED_timer_r;                          // Write the LED

//linkerarm
Ticker      LED_timer_l;                          // Write the LED

//Define HIDscope
//HIDScope    scope(2);                           //instantize a 2-channel HIDScope object


//Define filters and define the floats which contains the values.
BiQuadChain bqc;
BiQuad bq2_high(0.875182, -1.750364, 0.87518, -1.73472, 0.766004);
BiQuad bq3_notch(-1.1978e-16, 0.9561, 0.9780, -1.1978e-16, 0.9780);
BiQuad bq1_low(3.65747e-2, 7.31495e-2, 3.65747e-2, -1.390892, 0.537191);



// Rechterarm
double emgFiltered_r;
double filteredAbs_r;
double emg_value_r;
double onoffsignal_r;
bool check_calibration_r=0;
double avg_emg_r;
bool rechterarm_positief_r = false;
bool rechterarm_negatief_r = false;

//Linkerarm 
double emgFiltered_l;
double filteredAbs_l;
double emg_value_l;
double onoffsignal_l;
bool check_calibration_l=0;
double avg_emg_l;
bool linkerarm_positief_l = false;
bool linkerarm_negatief_l = false;

//                      FUNCTIONS  

//Rechterarm              
//function for filtering

void filter_r(){                                      
    if(check_calibration_r==1){
        
        
        
        
        emg_value_r = emg_r.read();
        emgFiltered_r = bqc.step( emg_value_r );
        filteredAbs_r = abs( emgFiltered_r );
    if (avg_emg_r != 0){
        onoffsignal_r=filteredAbs_r/avg_emg_r;             //divide the emg_r signal by the max emg_r to calibrate the signal per person
      }
       // scope.set(0,emg_value_r);                     //set emg_r signal to scope in channel 1
       //scope.set(1,onoffsignal_r);                   //set filtered signal to scope in channel 2
      //  scope.send();                               //send the signals to the scope
//            pc.printf("emg_r signal %f, filtered signal %f \n",emg_value_r,onoffsignal_r);
    }
}

//Linkerarm
//function for filtering

void filter_l(){                                      
    if(check_calibration_l==1){
        
        
        
        
        emg_value_l = emg_l.read();
        emgFiltered_l = bqc.step( emg_value_l );
        filteredAbs_l = abs( emgFiltered_l );
    if (avg_emg_l != 0){
        onoffsignal_l=filteredAbs_l/avg_emg_l;             //divide the emg_r signal by the avg_emg_l wat staat voor avg emg in gespannen toestand
      }
 
    }
}


//Rechterarm
//function to check the threshold
void check_emg_r(){           
double filteredAbs_temp_r;
          
    if((check_calibration_l==1) &&(check_calibration_r==1)){ 
    for( int i = 0; i<500;i++){
               filter_r();
               filteredAbs_temp_r = filteredAbs_temp_r + onoffsignal_r;
               wait(0.0004);
               }    
               filteredAbs_temp_r = filteredAbs_temp_r/500;                                 
            if(filteredAbs_temp_r<=0.3){                         //if signal is lower then 0.5 the blue light goes on
                 led1.write(1);          //led 1 is rood en uit
                    
                    rechterarm_positief_r = false;
                    rechterarm_negatief_r = true;
                   
            }
            else if(filteredAbs_temp_r > 0.3){                //if signal does not pass threshold value, blue light goes on
                    led1.write(0);
                    
                    rechterarm_negatief_r = false;
                    rechterarm_positief_r = true;
            }
            
    }
}

//Linkerarm
//function to check the threshold
void check_emg_l(){           
double filteredAbs_temp_l;
          
    if((check_calibration_l)==1 &&(check_calibration_r==1) ){ 
    for( int i = 0; i<500;i++){
               filter_l();
               filteredAbs_temp_l = filteredAbs_temp_l + onoffsignal_l;
               wait(0.0004);
               }    
               filteredAbs_temp_l = filteredAbs_temp_l/500;                                 
            if(filteredAbs_temp_l<=0.3){                         //if signal is lower then 0.5 the blue light goes on
             //    led1.write(1);          //led 1 is rood en uit
                   led2.write(1);          //led 2 is blauw en aan
                    linkerarm_positief_l = false;
                    linkerarm_negatief_l = true;
                   
            }
            else if(filteredAbs_temp_l > 0.3){                //if signal does not pass threshold value, blue light goes on
                 //   led1.write(0);
                   led2.write(0);
                    linkerarm_negatief_l = false;
                    linkerarm_positief_l = true;
            }
            
    }
}


//rechterarm
//function to calibrate
int calibration_r(){
  
                                            
   // if(button1.read()==false){
    
        
        
        led3.write(0);
       
        double signal_verzameling_r = 0;
        for(int n =0; n<5000;n++){   
           filter_r();
                              //read for 5000 samples as calibration
           emg_value_r = emg_r.read();
            emgFiltered_r = bqc.step( emg_value_r );
            filteredAbs_r = abs(emgFiltered_r);
         
          
           // signal_verzameling[n]=  filteredAbs_r;
             signal_verzameling_r = signal_verzameling_r + filteredAbs_r ;
             wait(0.0004);
             
              if (n == 4999){
                  avg_emg_r = signal_verzameling_r / n;
                  
                  }
            }  
          
             led3.write(1);
                 pc.printf("calibratie rechts compleet\n\r");
           // double lengte_array = sizeof(signal_verzameling);
      //     pc.printf("lengte_array = %f\n\r",lengte_array);
            
        //    for(int i = 0; i < lengte_array; i++){
      
     
      //    sum_array = sum_array + signal_verzameling[i] ;
      //      }
//avg_emg_r = sum_array / lengte_array;
     //   pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
        
        
        
        
        check_calibration_r=1;
        
   // }
    return 0;  
}

//linkeram
//function to calibrate
int calibration_l(){
  
                                            
   // if(button1.read()==false){
    
        
        
        led3.write(0);
       
        double signal_verzameling_l= 0;
        for(int n =0; n<5000;n++){   
           filter_l();
                              //read for 5000 samples as calibration
           emg_value_l = emg_l.read();
            emgFiltered_l = bqc.step( emg_value_l );
            filteredAbs_l = abs(emgFiltered_l);
         
          
           // signal_verzameling[n]=  filteredAbs_r;
             signal_verzameling_l = signal_verzameling_l + filteredAbs_l ;
             wait(0.0004);
             
              if (n == 4999){
                  avg_emg_l = signal_verzameling_l / n;
                  
                  }
            }  
          
            
        
      
     
      //    sum_array = sum_array + signal_verzameling[i] ;
      //      }
//avg_emg_r = sum_array / lengte_array;
     //   pc.printf("avg_emg_r = %f\n\r",avg_emg_r);
        
        
        
        
        check_calibration_l=1;
        led3.write(1);
        pc.printf("calibratie links compleet\n\r");
   // }
    return 0;  
}
 
//                         MAIN                      
 
int main(){
     pc.baud(115200);

pc.printf("Lampjes zijn langs geweest");
    led1.write(1);
    led2.write(1);
    led3.write(1);
    
    
    
    bqc.add( &bq1_low ).add( &bq2_high ).add( &bq3_notch );

  
    LED_timer_r.attach(&check_emg_r, 0.1);         //continously execute the motor controller
    LED_timer_l.attach(&check_emg_l, 0.1);         //continously execute the motor controller

    
    calibration_r();
    calibration_l();
    
    while(1){     //while loop to keep system going 
                                                        
    }     
}