File content as of revision 4:329e1022cbd3:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"

#define SAMPLING_RATE 0.002

MODSERIAL pc(USBTX,USBRX); 
AnalogIn emg1(A0); // onderste board
AnalogIn emg2(A1); // 2e board
AnalogIn emg3(A2); // 3e board
AnalogIn emg4(A3); // bovenste board

DigitalOut red(LED_RED);
DigitalOut green(LED_GREEN);
DigitalOut blue(LED_BLUE);


Ticker sample_timer; // naam van de emg-ticker

//HIDScope scope(4);  // aantal kanalen voor je HIDScope


int muscle;
double tijd;
double y5;


//highpass filter 20 Hz
const double numhigh_1 = 0.956543225556877;
const double numhigh_2 = -1.91308645113754;
const double numhigh_3 = 0.956543225556877;
const double denhigh_2 = -1.91197067426073;
const double denhigh_3 = 0.9149758348014341;

//biquad 1
const double notch1gain = 1.000000;
const double notch1_b0 = 1;
const double notch1_b1 = -1.61816176147 * notch1gain;
const double notch1_b2 = 1.00000000000 * notch1gain;
const double notch1_a1 = -1.58071559235 * notch1gain;
const double notch1_a2 = 0.97319685401 * notch1gain;
 
//biquad 2
const double notch2gain = 0.973674;
const double notch2_b0 = 1 * notch2gain;
const double notch2_b1 = -1.61816176147 * notch2gain;
const double notch2_b2 = 1.00000000000 * notch2gain;
const double notch2_a1 = -1.61244708381 * notch2gain;
const double notch2_a2 = 0.97415116257 * notch2gain;

//lowpass filter 7 Hz  - envelop
const double numlow_1 = 0.000119046743110057;
const double numlow_2 = 0.000238093486220118;
const double numlow_3 = 0.000119046743110057;
const double denlow_2 = -1.968902268531908;
const double denlow_3 = 0.9693784555043481;

//EMG variables
double emg_biceps; double biceps_power; double bicepsMVC = 0;
double emg_triceps; double triceps_power; double tricepsMVC = 0;
double emg_flexor; double flexor_power; double flexorMVC = 0;
double emg_extens; double extens_power; double extensMVC = 0;


// storage variables definieren
double f1_v1 = 0, f1_v2 = 0, f2_v1 = 0, f2_v2 = 0, f3_v1 = 0, f3_v2 = 0, f4_v1 = 0, f4_v2 = 0;

double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2)

{
    double v = u  - a1*v1 - a2*v2;
    double y = b0*v + b1*v1 + b2*v2;
    v2=v1;
    v1=v;
    return y;
    }

        
        
void sample_filter()
{
    
// double u1=..., u2...;
double u1 = emg1.read();
double y1 = biquad(u1, f1_v1, f1_v2, denhigh_2, denhigh_3, numhigh_1, numhigh_2, numhigh_3);
double y2 = biquad(y1, f2_v1, f2_v2, notch1_a1, notch1_a2, notch1_b0, notch1_b1, notch1_b2);
double y3 = biquad(y2, f3_v1, f3_v2, notch2_a1, notch2_a2, notch2_b0, notch2_b1, notch2_b2);
double y4 = abs(y3);
double y5 = biquad(y4, f4_v1, f4_v2, denlow_2, denlow_3, numlow_1, numlow_2, numlow_3);

    biceps_power=y5;
    //scope.set(0,u1);
    //scope.set(1,y4);
    //scope.set(2,biceps_power);
    //scope.send();
    
}


//Start sampling
void start_sampling(void)
{
    sample_timer.attach(&sample_filter,0.002);   //500 Hz EMG 
    green=0.5; blue=0.5;
    pc.printf("|-- started sampling --| \r\n");
}

//stop sampling
void stop_sampling(void)
{
    sample_timer.detach();
    green=1; blue=1;
    pc.printf("|-- stopped sampling --| \r\n");
}
     
void calibrate_emg()
{  
    //double sampletime=0;
    //sampletime=+SAMPLE_RATE;
    //
    // if(sampletime<5)
    //int muscle=1;
    //for(int index=0; index<2500;index++){   //measure 5 seconds@500hz = 2500 samples
     
        if(muscle==1){
            
            if(biceps_power>bicepsMVC){
            printf("+ ");
            bicepsMVC=biceps_power;
            }    
            else
            printf("- ");
        }  
        
        if(muscle==2){
            
            if(triceps_power>tricepsMVC){
            tricepsMVC=triceps_power;
            }    
        }
        
        if(muscle==3){
            
            if(flexor_power>flexorMVC){
            flexorMVC=flexor_power;
            }    
        }
        
        if(muscle==4){
            
            if(extens_power>extensMVC){
            extensMVC=extens_power;
            }    
        }
        
    //}
    tijd = tijd + 0.002;
    
    
   
}

int main()
{
   pc.baud(115200);
   Ticker timer;
   red=1; green=1; blue=1;
   
   pc.printf("Robot Started \r\n");
   pc.printf("Testcode calibration \r\n");
   wait(1);
   pc.printf("+ means current sample is higher than stored MVC\r\n");
   pc.printf("- means current sample is lower than stored MVC\r\n");
   wait(3);
   pc.printf(" Press any key to begin... "); wait(1);
   char input;
   input=pc.getc();
   pc.putc(input); 
   pc.printf(" \r\n  Starting in 3... \r\n"); wait(1);
   pc.printf(" \r\n  Starting in 2... \r\n"); wait(1);
   pc.printf(" \r\n  Starting in 1... \r\n"); wait(1);
   
   start_sampling();
   muscle=1;
   timer.attach(&calibrate_emg,0.002);
   wait(5);
   timer.detach();
   
   pc.printf("\r\n Muscle MVC = %f \r\n",bicepsMVC);
   
   pc.printf("Calibrate_emg() exited \r\n");
   pc.printf("Measured time: %f seconds \r\n",tijd);
   
   // repeat for other muscles.
   
   //stop_sampling();
   while (true){   
        
        wait(1);
    }
}