File content as of revision 52:45200bbde108:

#include "mbed.h"
#include "HIDScope.h"
#include "MODSERIAL.h"
#include "biquadFilter.h" //Filter direct form II
#include "QEI.h"
const double ledon = 1;
const double ledoff = 0;

DigitalOut ledgreen1(D0);
DigitalOut ledgreen2(D1);
DigitalOut ledyellow1(D2);
DigitalOut ledyellow2(D3);
DigitalOut ledred1(D9);
DigitalOut ledred2(D10);

void leduit() { ledgreen1 = ledoff; ledgreen2 = ledoff; ledyellow1 = ledoff; ledyellow2 = ledoff; ledred1 = ledoff; ledred2 = ledoff; }

//      [DEFINE INPUTS]       //
DigitalOut  debug_led_red  (LED_RED);                 // Debug LED
DigitalOut  debug_led_green(LED_GREEN);             // Debug LED 
DigitalOut  debug_led_blue (LED_BLUE);               // Debug LED

const double  off=1;                            // Led off
const double  on=0;                             // Led on

// MOTORS               
QEI          motor_turn(D12,D13,NC,32);                           QEI        motor_strike(D9,D10,NC,32);                                // TURN - STRIKE : Encoder for motor
PwmOut       pwm_motor_turn(D5);                                  PwmOut     pwm_motor_strike(D6);                                      // TURN - STRIKE : Pwm for motor
DigitalOut   motordirection_turn(D4);                             DigitalOut motordirection_strike(D7);                                 // TURN - STRIKE : Direction of the motor
 

//      [DEFINE CONSTANTS]      //
HIDScope scope(4); // Aantal HIDScope kanalen
MODSERIAL pc(USBTX,USBRX);
Ticker control_tick;
Ticker T1;

//      [BIQUAD FILTERS]        //
int Fs = 512; // sampling frequency
const double low_b0 = 0.05892937945281792;
const double low_b1 = 0.11785875890563584;
const double low_b2 = 0.05892937945281792;
const double low_a1 = -1.205716572226748;
const double low_a2 = 0.44143409003801976; //Notch 1 LOW: VIA online biquad calculator Lowpass 520-48-0.7071-6

const double high_b0 = 0.6389437261127494;
const double high_b1 = -1.2778874522254988;
const double high_b2 = 0.6389437261127494;
const double high_a1 = -1.1429772843080923;
const double high_a2 = 0.41279762014290533; // NOTCH 2 HIGH: VIA online biquad calculator Highpass 520-52-0.7071-6

const double high_b0_HP = 0.84855234544818812;
const double high_b1_HP = -1.6970469089637623;
const double high_b2_HP = 0.8485234544818812;
const double high_a1_HP = -1.6564788473046559;
const double high_a2_HP = 0.7376149706228688; // HIGHPASS : NOG OPZOEKEN!! : >25Hz? sample rate 512Hz

const double low_b0_LP = 0.0013067079602315681;
const double low_b1_LP = 0.0026134159204631363;
const double low_b2_LP = 0.0013067079602315681;
const double low_a1_LP = -1.9238184798980429;
const double low_a2_LP =  0.9290453117389691; // LOWPASS : NOG OPZOEKEN!! <5-10 Hz? sample rate 512Hz
//
//Left bicep
biquadFilter highpassfilter_1(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden? (>25Hz doorlaten)
biquadFilter notchL1(high_a1, high_a2, high_b0, high_b1, high_b2);// moeten nog waardes voor gemaakt worden (>52Hz doorlaten)
biquadFilter notchL2(low_a1, low_a2, low_b0, low_b1, low_b2);// moeten nog waardes voor gemaakt worden (<48Hz doorlaten)
biquadFilter lowpassfilter_1(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP); 

// Right bicep
biquadFilter highpassfilter_2(high_a1_HP, high_a2_HP, high_b0_HP, high_b1_HP, high_b2_HP);// moeten nog waardes voor gemaakt worden?
biquadFilter notchR1(high_a1, high_a2, high_b0, high_b1, high_b2);  // moeten nog waardes voor gemaakt worden
biquadFilter notchR2(low_a1, low_a2, low_b0, low_b1, low_b2); // moeten nog waardes voor gemaakt worden
biquadFilter lowpassfilter_2(low_a1_LP, low_a2_LP, low_b0_LP, low_b1_LP, low_b2_LP);// moeten nog waardes voor gemaakt worden?


AnalogIn input1(A0);                                          // EMG: Two AnalogIn EMG inputs
AnalogIn input2(A1);                                          // EMG: Two AnalogIn EMG inputs

//                     __________________________
//                    :     [EMG variables]      : 
//                    :__________________________:
//

volatile bool control_go = false;                             // EMG: 
volatile bool sample = false;
volatile bool sample_error = false;
volatile bool sample_error_strike = false;

double Threshold_Bicep_Left_1,  Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2;     //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);


double minimum_L;                      double maximum_L;                                           
double EMG_L_min;                      double EMG_L_max;                                    
double minimum_R;                      double maximum_R;                                              
double EMG_R_min;                      double EMG_R_max;                                                                                                                 
double EMG_left_Bicep;                 double EMG_Right_Bicep;                   // input variables
double EMG_Left_Bicep_filtered_notch_1;double EMG_Right_Bicep_filtered_notch_1; 
double EMG_Left_Bicep_filtered_notch_2;double EMG_Right_Bicep_filtered_notch_2; 
double EMG_Left_Bicep_filtered;        double EMG_Right_Bicep_filtered;          // output variables


double n=0; double c=0; double k=0; double p=0; double er=0;

// FUNCTIONS
void ControlGo();
void take_sample();
void Filter();
void sample_filter();
void countdown_from_5();
void calibration();
void red();void blue();void green();void white();void yellow();void cyan();void purple(); void black();

double RS_0,RS_1,RS_2,RS_3,RS_4,RS_5,RS_6,RS_7,RS_8,RS_9,RS_10,
       RS_11,RS_12,RS_13,RS_14,RS_15,RS_16,RS_17,RS_18,RS_19,RS_20,
       RS_21,RS_22,RS_23,RS_24,RS_25,RS_26,RS_27,RS_28,RS_29,RS_30,RS_31;
       
double LS_0,LS_1,LS_2,LS_3,LS_4,LS_5,LS_6,LS_7,LS_8,LS_9,LS_10,
       LS_11,LS_12,LS_13,LS_14,LS_15,LS_16,LS_17,LS_18,LS_19,LS_20,
       LS_21,LS_22,LS_23,LS_24,LS_25,LS_26,LS_27,LS_28,LS_29,LS_30,LS_31;
       
double moving_average_right, moving_average_left;

void keep_in_range                  (double * in, double min, double max);  

double f; double pwm_average; double smp;
double position_strike, position_turn, pwm_strike, pwm_turn;
double        conversion_counts_to_degrees=0.085877862594198; 
//==========================//
//      [MAIN FUNCTION]     //
//==========================//
int main()
{
    control_tick.attach(&ControlGo, (float)1/Fs);
    pc.baud(115200);
    //double n=0;
    //calibration(); // calibreer min en max positie
    f=1;
    smp=0;
    while(1)
    {
        if(f==1)
        {
        pwm_motor_strike=0;
        pwm_motor_turn=0; 
        pc.printf("start /n");            
        }
        if(f==512)
        {
        pwm_motor_strike=1;
        motordirection_strike=1;
        pwm_motor_turn=1;   
        motordirection_turn=1;
        }
        
        if(f==2048)
        {
        pc.printf("stop /n");
        pwm_motor_strike=0;
        pwm_motor_turn=0;    
        }
        
        if(f==2560) // same cycle but referesed direction
        {
        pwm_motor_strike=0;
        pwm_motor_turn=0; 
        pc.printf("start /n");            
        }
        if(f==3072)
        {
        pwm_motor_strike=1;
        motordirection_strike=0;
        pwm_motor_turn=1;   
        motordirection_turn=0;
        }
        
        if(f==4500)
        {
        pc.printf("stop /n");
        pwm_motor_strike=0;
        pwm_motor_turn=0;
        f=0;    
        }
        
        yellow();
        if(control_go)
        { 
        control_go=0;
        f++;
        
        if ((motor_turn.getPulses()>4200) || (motor_turn.getPulses()<-4200))                   // If value exceeds -4200 and 4200 (number of counts equal to one revolution) than reset to zero
                { motor_turn.reset(); } 
        
         position_turn = conversion_counts_to_degrees * motor_turn.getPulses();                 // Convert counts to degrees
         pwm_strike=pwm_motor_strike;
         pwm_turn=pwm_motor_turn;
         
         if ((motor_strike.getPulses()>4200) || (motor_strike.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
                { motor_strike.reset(); } 
        
         position_strike = conversion_counts_to_degrees * motor_strike.getPulses();
         
            scope.set(0,pwm_strike);                  //ik weet niet of dit weergegeven kan worden
            scope.set(1,pwm_turn);                    //Filtered signal
            scope.set(2,position_turn);             //
            scope.set(3,position_strike);             //
            scope.send();
        }
        
        
            
            
//        double signal_above_threshold=(moving_average_left); //(moving_average_right-Threshold_Bicep_Right_1)+
//        double max_signal=(EMG_L_max); //(EMG_R_max-Threshold_Bicep_Right_1)+
//        pwm_strike=signal_above_threshold/max_signal;
//        keep_in_range(&pwm_strike, 0,1); 
//        pwm_strike=pwm_strike*pwm_strike;
//        
//        pwm_average=pwm_strike+pwm_average/f;
//        f++;
//        smp++;
//        pc.printf("Pwm=%f \n\r", pwm_average);
//        double speed=fabs(pwm_average);
//        
//        
//if (pwm_average < 0.1)   {   leduit();            }
//if (pwm_average > 0.1)   {   ledgreen1  = ledon;  }
//if (pwm_average > 0.2)   {   ledgreen2  = ledon;  }
//if (pwm_average > 0.3)   {   ledyellow1 = ledon;  }
//if (pwm_average > 0.5)   {   ledyellow2 = ledon;  }
//if (pwm_average > 0.7)   {   ledred1    = ledon;  }
//if (pwm_average > 0.9)   {   ledred2    = ledon;  } 

  //      if(smp>512)
//        {
//        pwm_motor_strike=speed;
//        pwm_motor_turn=speed;
//        green();
//        pwm_motor_strike=0;
//        wait(3);
//        f=1;
//        smp=0;}
//        }
    } // while end   
} // main end

// --------------------------------------------------------------------------------------------------------
//      [FUNCTIONS]        //
void ControlGo() //Control flag
{
    control_go = true;
}

void sample_filter()
{
    Filter();
    take_sample();
    if(sample)
        {
            sample=false;
            LS_0 = EMG_Left_Bicep_filtered;               RS_0 = EMG_Right_Bicep_filtered; 
            
            LS_30=LS_29; LS_29=LS_28;LS_28=LS_27; LS_27=LS_26; LS_26=LS_25; LS_25=LS_24; LS_24=LS_23; LS_23=LS_22; LS_22=LS_21; LS_20=LS_19;
            LS_19=LS_18; LS_18=LS_17;LS_17=LS_16; LS_16=LS_15; LS_15=LS_14; LS_14=LS_13; LS_13=LS_12; LS_12=LS_11; LS_11=LS_10; LS_10=LS_9;
            LS_9=LS_8; LS_8=LS_7;LS_7=LS_6; LS_6=LS_5; LS_5=LS_4; LS_4=LS_3; LS_3=LS_2; LS_2=LS_1; LS_1=LS_0; 
            
            RS_30=RS_29; RS_29=RS_28;RS_28=RS_27; RS_27=RS_26; RS_26=RS_25; RS_25=RS_24; RS_24=RS_23; RS_23=RS_22; RS_22=RS_21; RS_20=RS_19;
            RS_19=RS_18; RS_18=RS_17;RS_17=RS_16; RS_16=RS_15; RS_15=RS_14; RS_14=RS_13; RS_13=RS_12; RS_12=RS_11; RS_11=RS_10; RS_10=RS_9;
            RS_9=RS_8; RS_8=RS_7;RS_7=RS_6; RS_6=RS_5; RS_5=RS_4; RS_4=RS_3; RS_3=RS_2; RS_2=RS_1; RS_1=RS_0; 
            
        }
        moving_average_left= (LS_30+LS_29+LS_28+LS_27+LS_26+LS_25+LS_24+LS_23+LS_22+LS_21+LS_20+LS_19+LS_18+LS_17+LS_16+LS_15+LS_14+LS_13+LS_12+LS_11+LS_10+LS_9+LS_8+LS_7+LS_6+LS_5+LS_4+LS_3+LS_2+LS_1+LS_0)/31;
        moving_average_right= (RS_30+RS_29+RS_28+RS_27+RS_26+RS_25+RS_24+RS_23+RS_22+RS_21+RS_20+RS_19+RS_18+RS_17+RS_16+RS_15+RS_14+RS_13+RS_12+RS_11+RS_10+RS_9+RS_8+RS_7+RS_6+RS_5+RS_4+RS_3+RS_2+RS_1+RS_0)/31;
    n++;   
}

void take_sample() // Take a sample every 25th sample
{
    if(n==8)
    {
    sample = true; n=0;
    }
    
     if(er==5)
    {
    sample_error = true; er=0;
    }
    
    sample_error_strike = true;
}

                                //    [FILTER FUNCTIONS]    //
                                //          [EMG]           //
                                  
void Filter() // Unfiltered EMG (input) -> highpass filter -> rectify -> lowpass filter -> Filtered EMG (output)
    {
        
     //ZONDER NOTCH 
    EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
    
    EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
    EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);

    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
    
    
//    EMG_left_Bicep = input1; EMG_Right_Bicep = input2;
//    
//    EMG_Left_Bicep_filtered = highpassfilter_1.step(EMG_left_Bicep); EMG_Right_Bicep_filtered = highpassfilter_2.step(EMG_Right_Bicep);
//    
//    EMG_Left_Bicep_filtered_notch_1 = notchL1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered_notch_1 = notchR1.step(EMG_Right_Bicep_filtered);
//    EMG_Left_Bicep_filtered_notch_2 = notchL2.step(EMG_Left_Bicep_filtered_notch_1); EMG_Right_Bicep_filtered_notch_2 = notchR2.step(EMG_Right_Bicep_filtered_notch_1);
//    
//    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered_notch_2); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered_notch_2);
//    
//    
//    EMG_Left_Bicep_filtered = fabs(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = fabs(EMG_Right_Bicep_filtered);
//    
//    EMG_Left_Bicep_filtered = lowpassfilter_1.step(EMG_Left_Bicep_filtered); EMG_Right_Bicep_filtered = lowpassfilter_2.step(EMG_Right_Bicep_filtered);
    }
    
void countdown_from_5() // Countdown from 5 till 0 inside Putty (interface)
    {
    wait(1); pc.printf("5 \n\r"); wait(1); pc.printf("4 \n\r"); wait(1); pc.printf("3 \n\r"); wait(1); pc.printf("2 Ready \n\r");
    wait(1); pc.printf("1 Set \n\r"); wait(1); pc.printf("Go \n\r");
    }
    
void calibration()
{
    
                        //  [MINIMUM VALUE BICEPS CALIBRATION]   //

    pc.printf("Start minimum calibration in 5 seconds \n\r");
    pc.printf("Keep your biceps as relaxed as possible \n\r");
    
    countdown_from_5();
    c=0;
    
    while(c<2560)                    // 512Hz -> 2560 is equal to five seconds
    {
    Filter();                        // Filter EMG signal
    minimum_L=EMG_Left_Bicep_filtered+minimum_L;          // Take previous sample EMG_Left_Bicep_filtered and add the new value
    minimum_R=EMG_Right_Bicep_filtered+minimum_R;   
//    scope.set(0,EMG_left_Bicep);
//    scope.set(1,EMG_Left_Bicep_filtered);
//    scope.set(2,minimum_L);
//    scope.send();
    c++;                             // Every sample c is increased by one until the statement c<2560 is false
    wait(0.001953125);               // wait one sample
    }
    
    pc.printf("Finished minimum calibration \n\r");
    
    EMG_L_min=minimum_L/2560;        // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
    EMG_R_min=minimum_R/2560;
    
    pc.printf("EMG_L_min = %f \n\r EMG_R_min = %f \n\r", EMG_L_min, EMG_R_min);

    wait (3);                        //cooldown
    
    
                    //  [MAXIMUM VALUE BICEPS CALIBRATION]   //
                    
                    
    pc.printf("start maximum calibration in 5 seconds (start contraction at 3) \n\r");
    
    countdown_from_5();
    c=0;
    
    while(c<2560)              // 512Hz -> 2560 is equal to five seconds
    {
    Filter();                  // Filter EMG signal
    maximum_L=EMG_Left_Bicep_filtered+maximum_L;    // Take previous sample EMG_Left_Bicep_filtered and add the new value
    maximum_R=EMG_Right_Bicep_filtered+maximum_R;     
    c++;                       // Every sample c is increased by one until the statement c<2560 is false
    wait(0.001953125);
    }
    
    pc.printf("Finished minimum calibration \n\r");
    
    EMG_L_max=maximum_L/2560; // Divide the summation by the number of measurements (2560 measurements) to get a mean value over 5 seconds
    EMG_R_max=maximum_R/2560;
    
    pc.printf("EMG_L_max = %f \n\r EMG_R_max = %f  \n\r", EMG_L_max, EMG_R_max);

    wait (3); //cooldown
    
    
                     //  [MAXIMUM VALUE BICEPS CALIBRATION]   //
                     //   Calculate threshold percentages     //
                     
    Threshold_Bicep_Left_1=((EMG_L_max-EMG_L_min)*0.2);      //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // LEFT
    Threshold_Bicep_Left_2=((EMG_L_max-EMG_L_min)*0.6);      //(waarde waarop het gemeten EMG signaal 60% van max het maximale is); 
    Threshold_Bicep_Right_1=((EMG_R_max-EMG_R_min)*0.2);     //(waarde waarop het gemeten EMG signaal 20% van max het maximale is); // RIGHT
    Threshold_Bicep_Right_2=((EMG_R_max-EMG_R_min)*0.6);     //(waarde waarop het gemeten EMG signaal 60% van max het maximale is);

    pc.printf("left 1: %f left 2: %f right 1: %f right 2: %f  \n\r", Threshold_Bicep_Left_1, Threshold_Bicep_Left_2, Threshold_Bicep_Right_1, Threshold_Bicep_Right_2);
    
}

void red()      { debug_led_red=on;     debug_led_blue=off;  debug_led_green=off;   }
void blue()     { debug_led_red=off;    debug_led_blue=on;   debug_led_green=off;   }
void green()    { debug_led_red=off;    debug_led_blue=off;  debug_led_green=on;    }
void white()    { debug_led_red=on;     debug_led_blue=on;   debug_led_green=on;    }
void yellow()   { debug_led_red=on;     debug_led_blue=off;  debug_led_green=on;    }
void cyan()     { debug_led_red=off;    debug_led_blue=on;   debug_led_green=on;    }
void purple()   { debug_led_red=on;     debug_led_blue=on;   debug_led_green=off;   }
void black()    { debug_led_red=off;    debug_led_blue=off;  debug_led_green=off;   }

void keep_in_range(double * in, double min, double max)                             // Put in certain min and max values that the input needs to stay within
{
    *in > min ? *in < max? : *in = max: *in = min;
}