File content as of revision 43:7d0b2dc05b80:

//libraries
#include "mbed.h"               //mbed library
#include "HIDScope.h"           //hidscope library
#include "QEI.h"                //library for encoder functions
#include "BiQuad.h"             //library for filtering with BiQuad
#include "MODSERIAL.h"          //library for connect pc with mbed

//Define objects

    //EMG
    AnalogIn    emg_biceps_right_in( A0 );              //analog in to get EMG biceps  (r) in to c++
    AnalogIn    emg_triceps_right_in(A1);               //analog in to get EMG triceps (r) in to c++
    AnalogIn    emg_biceps_left_in  (A2);               //analog in to get EMG biceps  (l) in to c++

    //Encoder
    DigitalIn encoder1A(D13);
    DigitalIn encoder1B(D12);
    DigitalIn encoder2A(D11); 
    DigitalIn encoder2B(D10);

    //callibration button
    InterruptIn   button_calibration_biceps (SW3);                //button to start calibration biceps
    InterruptIn   button_calibration_triceps (SW2);               // button to start calibration tricps

    // reset button
    DigitalIn resetbutton(D9);
    
    //tickers

    Ticker      sample_timer;               //ticker for emg sampling
    Ticker      switch_function;            //ticker for switch
    Ticker      speed_measuring;            //ticker for speed measurment

    //everything for monitoring
    HIDScope    scope(5);                   //open 3 channels in hidscope
    MODSERIAL   pc(USBTX, USBRX);            //pc connection
    DigitalOut  red(LED_RED);
    DigitalOut  green(LED_GREEN);
    DigitalOut  blue(LED_BLUE);

    //motors
    DigitalOut  direction_motor1(D4);
    PwmOut      pwm_motor1(D5);
    DigitalOut  direction_motor2(D7);
    PwmOut      pwm_motor2(D6);

//define variables

    //for motorcontrol
    const int cw = 0;                 // motor should turn clockwise
    const int ccw =1;                  // motor should turn counterclockwise
    const float gearboxratio=131.25;    // gearboxratio from encoder to motor
    const float rev_rond=64.0;          // revolutions per round of encoder
    int    onoffsignal_biceps=0;        // on/off signal: 1; biceps activation, 0: nothing, -1, triceps activation
    int    switch_signal_triceps=0;     // switching between motors. 
    
    volatile double cut_off_value_biceps_right =    0.04;       //tested, normal values. Can be changed by calibration
    volatile double cut_off_value_biceps_left  =    -0.04;      //volatiles becaused changen in interrupt
    volatile double cut_off_value_triceps=-0.03;       
    double signal_biceps_sum;
    double bicepstriceps_rightarm;

    volatile double voltage_motor1=0.18; //pwm is de pulse with tussen geen ampere en wel ampere motor 1
    volatile double voltage_motor2=1;//pwm is de pulse with tussen geen ampere en wel ampere motor 1
         
    int motorswitch= 0; //start van de teller wordt op nul gesteld

    //variables and constants for calibration
    
    const float percentage_max_triceps=0.3;
    const float percentage_max_biceps =0.3;
    double max_biceps;                          //calibration maximum biceps
    double max_triceps;                         //calibration maximum triceps
    
    //variables for feedback loop, manual calibration
    
    volatile double current_position_motor1 = 0;                // at start, the position is 0. Manual calibration
    volatile double previous_position_motor1 = 0;               // at start, the position is 0. Manual calibration                 
    volatile double current_position_motor2 = 0;                // at start, the position is 0. Manual calibration 
    volatile double previous_position_motor2 = 0;               // at start, the position is 0. Manual calibration 
    volatile double rev_counts_motor1=0;
    volatile double rev_counts_motor2=0;
    volatile double counts_encoder1;
    volatile double counts_encoder2;
                            
    volatile bool   tickerflag;                        //tickerflag is true or false, implicated by bool. 
    volatile double speed_motor1;                    // speed in rad/s
    volatile double speed_motor2;                    // speed in rad/s
    
    
    //speedmeasuring 
    double ticker_TS=0.001;    // time step to derivate position to speed, in seconds. 
    volatile double timepassed=0;                       //de waarde van hoeveel tijd er verstreken is
    
    //resetbuttons
    volatile double value1_resetbutton = 0;             
    volatile double value2_resetbutton = 0;
    
    //filter defining
    
    //b1 = biceps right arm
    BiQuad filterhigh_b1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
    BiQuad filternotch1_b1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
    
    //t1= triceps right arm
    BiQuad filterhigh_t1(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
    BiQuad filternotch1_t1 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
    
    //b2= biceps left arm
    BiQuad filterhigh_b2(9.5654e-01,-1.9131e+00,9.5654e-01,-1.9112e+00,9.1498e-01);
    BiQuad filternotch1_b2 (9.9376e-01 , -1.8902e-00,   9.9376e-01 , -1.8902e-00 ,  9.875e-01);
    
    //after abs filtering
    BiQuad filterlow_b1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
    BiQuad filterlow_t1 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);
    BiQuad filterlow_b2 (6.2942e-06, 1.2588e-05,6.2942e-06,-1.9929e+00,9.9292e-01);

//voids

    //to make a tickerfunction for speedmeasurment
    
    void speed_sampling()                // maakt een ticker functie aan
    {
    tickerflag = 1;                     // het enige wat deze functie doet is zorgen dat tickerflag 1 is
    }
    
    //void to make the switch between the motors with triceps
    
    void SwitchN() {                      
        if(switch_signal_triceps==1)
        {
            motorswitch++;
            
        if (motorswitch%2==0)  
          { pc.printf("If you contract the right arm, the robot will go right \r\n");
            pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
            pc.printf("\r\n");
            green=0;
            red=1;
           }
        
        else
            {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
            pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
            pc.printf("\r\n");
            green=1;
            red=0;
            }    
    
            }
                }
        
    //callibration
    void calibration_biceps(){
            pc.printf("start of calibration biceps, contract maximal \n");
            red=1;
            green=1;
            blue=0;
            
            for(int n =0; n<3000;n++)                                                  //read for 2000 samples as calibration
                    {
            double emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
            double  emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
            double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
            double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
            double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
                                    
                if (emg_filtered_biceps_right > max_biceps)                    //determine what the highest reachable emg signal is
                    {
                max_biceps = emg_filtered_biceps_right;
                
                    }
                    wait(0.001f); //to sample at same freq; 1000Hz
                    }
                cut_off_value_biceps_right=percentage_max_biceps*max_biceps; 
                cut_off_value_biceps_left=-cut_off_value_biceps_right;
                //toggle lights
                blue=!blue;
                
                pc.printf(" end of calibration\r\n",cut_off_value_biceps_right );   
                pc.printf(" change of cv biceps: %f ",cut_off_value_biceps_right );
                
                wait(0.5f);
                  
                if (motorswitch%2==0)
                {green=0;
                red=1;}
                
                else       {green=1;
                red=0;}
                
                    }
                    
    void calibration_triceps(){
            red=1;
            green=1;
            blue=0;
          
            pc.printf("start of calibration triceps\r\n");
    
            for(int n =0; n<3000;n++)                                                  //read for 2000 samples as calibration
                    {
            double emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
            double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
            double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
            double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
            double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
                                  
                if (emg_filtered_triceps_right > max_triceps)                    //determine what the highest reachable emg signal is
                    {
                max_triceps = emg_filtered_triceps_right;
                
                    }
                    wait(0.001f); //to sample at same freq; 1000Hz
                    }
                cut_off_value_triceps=-percentage_max_triceps*max_triceps; 
                pc.printf(" end of calibration\r\n");   
                pc.printf(" change of cv triceps: %f ",cut_off_value_triceps ); 
                blue=!blue;
                wait(0.5f);
                if (motorswitch%2==0)
                {green=0;
                red=1;}
                
                else       {green=1;
                red=0;}  
                
                    }
    
    
    //sampling emg with filters as defined before
    void filter(){
            //biceps right arm read+filtering
            double emg_biceps_right=emg_biceps_right_in.read();                            //read the emg value from the elektrodes
            double emg_filtered_high_biceps_right= filterhigh_b1.step(emg_biceps_right);
            double emg_filtered_high_notch_1_biceps_right=filternotch1_b1.step(emg_filtered_high_biceps_right);
            double emg_abs_biceps_right=fabs(emg_filtered_high_notch_1_biceps_right); //fabs because float
            double emg_filtered_biceps_right=filterlow_b1.step(emg_abs_biceps_right);
            
            //triceps right arm read+filtering
            double emg_triceps_right=emg_triceps_right_in.read();                            //read the emg value from the elektrodes
            double emg_filtered_high_triceps_right= filterhigh_t1.step(emg_triceps_right);
            double emg_filtered_high_notch_1_triceps_right=filternotch1_t1.step(emg_filtered_high_triceps_right);
            double emg_abs_triceps_right=fabs(emg_filtered_high_notch_1_triceps_right); //fabs because float
            double emg_filtered_triceps_right=filterlow_t1.step(emg_abs_triceps_right);
            
            //biceps left arm read+filtering
            double emg_biceps_left=emg_biceps_left_in.read();                            //read the emg value from the elektrodes
            double emg_filtered_high_biceps_left= filterhigh_b2.step(emg_biceps_left);
            double emg_filtered_high_notch_1_biceps_left=filternotch1_b2.step(emg_filtered_high_biceps_left);
            double emg_abs_biceps_left=fabs(emg_filtered_high_notch_1_biceps_left); //fabs because float
            double emg_filtered_biceps_left=filterlow_b2.step(emg_abs_biceps_left);
              
            //signal substraction of filter biceps and triceps. right Biceps + left biceps -
            signal_biceps_sum=emg_filtered_biceps_right-emg_filtered_biceps_left;
            bicepstriceps_rightarm= emg_filtered_biceps_right-emg_filtered_triceps_right;
                  
            //creating of on/off signal with the created on/off signals, with if statement   for right arm!    
            if (signal_biceps_sum>cut_off_value_biceps_right)
            {onoffsignal_biceps=1;}
              
            else if (signal_biceps_sum<cut_off_value_biceps_left)
            {
            onoffsignal_biceps=-1;
            }    
            
            else
            {onoffsignal_biceps=0;}
                          
            //creating on/off signal for switch (left arm)
            
            if (bicepstriceps_rightarm<cut_off_value_triceps)
            {
            switch_signal_triceps=1;    
            }    
            
            else
            {
            switch_signal_triceps=0;              
            }
            
            //send signals  to scope
            scope.set(0, emg_filtered_biceps_right);            //set emg signal to scope in channel 0
            scope.set(1, emg_filtered_triceps_right);           // set emg signal to scope in channel 1
            scope.set(2, emg_filtered_biceps_left);                     // set emg signal to scope in channel 2
       
            
            scope.send();                       //send all the signals to the scope
                    }

//program

int main()
{  
    pc.baud(115200); //connect with pc with baudrate 115200
    QEI Encoder2(D12,D13, NC, rev_rond,QEI::X4_ENCODING);  // maakt een encoder aan! D12/D13 ingangen, rev_rond zijn aantal pulsen per revolutie! Bovenaan in te stellen. 
    QEI Encoder1(D10,D11, NC, rev_rond,QEI::X4_ENCODING);

    //attach voids to tickers and interrupts
    speed_measuring.attach(&speed_sampling,ticker_TS);      //sampling function, for speed measurement 
    sample_timer.attach(&filter, 0.001);                    //continously execute the EMG reader and filter, it ensures that filter and sampling is executed every 1/frequency seconds
    switch_function.attach(&SwitchN,1.0);                   //switch is every second available
    button_calibration_biceps.fall (&calibration_biceps);   //to call calibration biceps, stop everything else
    button_calibration_triceps.fall(&calibration_triceps);  //to call calibration triceps, stop everything else

    //print which motor will be controlled by text and light. Red: up/down=motor 2. Green: left/right=motor 1.
      if (motorswitch%2==0)  
          { pc.printf("If you contract the right arm, the robot will go right \r\n");
            pc.printf("If you contract biceps of the left arm, the robot will go left \r\n");
            pc.printf("\r\n");
        green=0;
        red=1;
        blue=1;
          } //if loop closed
    
    else
        {pc.printf("If you contract the biceps of right arm, the robot will go up \r\n");
         pc.printf("If you contract the biceps of left arm, the robot will go down \r\n");
         pc.printf("\r\n");
        green=1;
        red=0;
        blue=1;
        }    //else loop closed


//endless loop

    while (true) {                        // zorgt er voor dat de code oneindig doorgelopen wordt  
   
   //encoder and velocity measurement
        if (tickerflag ==1)
            {
               //position change, 'memory function' 
       previous_position_motor1 = current_position_motor1; // zorgt er voor dat de huidige positie wordt gedefineerd als de vorige positie is
       current_position_motor1 = rev_counts_motor1;        // zorgt dat de huidige positie wordt gedefineerd als het huidige aantal rondejs dat gedraaid is
        previous_position_motor2 = current_position_motor2; // zelfde maar dan voor motor2
        current_position_motor2 = rev_counts_motor2;
       
        //speed calculation
        speed_motor1=((current_position_motor1 - previous_position_motor1)*6.28318530718) / ticker_TS;
        speed_motor2 = ((current_position_motor2 - previous_position_motor2)*6.28318530718) / ticker_TS; 
        
       tickerflag = 0; // reset de tickerflag weer op 0 zodat het loopje niet wordt doorlopen tot de volgende tick zo kan de tijd tussen het lopen van ieder loopje gecontroleerd worden
       }      //if tickerflag==1 closed
   
   //control and monitor motor with EMG signal, with build in restrictions.
  
    //left biceps contracted:
        if (onoffsignal_biceps==-1)        //left biceps contracted
        { //open if loop for left biceps
        
                if (motorswitch%2==0)             //-3.4 is limitationpoint, when motor turns clockwise
                { 
                direction_motor1 = cw;
                pwm_motor1 = voltage_motor1; 
                counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in  
                rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
                value1_resetbutton = 0;      
                 }  //if loop closed
                         
                else if (motorswitch%2!=0 && rev_counts_motor2<2.0) //2.0 is limitation for motor 2 when clockwise
                {
                direction_motor2 = cw;
                pwm_motor2 = voltage_motor2;
                counts_encoder2 = Encoder2.getPulses(); //tellen van de pulsen in  
                rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
                value2_resetbutton = 0;
                } //else if loop closed  
                 
        //speed control motor 1
            if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
            voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
            
            else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
            {         voltage_motor1 = voltage_motor1+0.005;  }
        
        //speed control motor 2
            if (fabs(speed_motor2) > 5.0)
            {        voltage_motor2 = voltage_motor2-0.005;       }
             
            else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
            {        voltage_motor2 = voltage_motor2+0.005;  }
        
     }          //if left biceps is contracted closed
    
    
    //right biceps contract (else if case)    
        else if (onoffsignal_biceps==1)                     //right biceps contracted
    {        
            if (motorswitch%2==0) //limitation of motor turning right                    
            {
            direction_motor1 = ccw; //turning right
            pwm_motor1 = voltage_motor1;
            counts_encoder1 = Encoder1.getPulses(); //tellen van de pulsen in  
            rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
            value1_resetbutton = 0;
            }  //if loop; motor turning right closed
        
          else if (motorswitch%2!=0 && rev_counts_motor2>-2.0) //limitation of motor turning down
            {
            direction_motor2 = ccw;
            pwm_motor2 = voltage_motor2;
            counts_encoder2 = Encoder2.getPulses();  
            rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);            //weergeven van het aantal rondjes
            value2_resetbutton = 0;    
           }        //if loop closed
                 
        //speed control motor 1
        if (fabs(speed_motor1) > 3.0){ // zorgt dat als de absolute van de snelheid van motor 1 boven de target snelheid zit ( in dit geval 3.0 rad/s) dat er in dit loopje gelopen wordt
        voltage_motor1 = voltage_motor1-0.005;} // zorgt er voor dat de pwm verlaagd word hierdoor word de puls lengte kleiner en zal de motor langzamer gaan draaien.
        
        else if (fabs(speed_motor1) < 3.0 && speed_motor1 != 0)
        {         voltage_motor1 = voltage_motor1+0.005;  }
        
        //speed control motor 2
         if (fabs(speed_motor2) > 5.0)
         {        voltage_motor2 = voltage_motor2-0.005;       }
         
        else if (fabs(speed_motor2) < 5.0 && speed_motor2 != 0)
        {        voltage_motor2 = voltage_motor2+0.005;  }
        
     }  //else if loop closed; right biceps contracted    
    
    else{           //no signal of both biceps!
    //encoders because even when signal off, motor can turn for a while. 
    
    counts_encoder1 = Encoder1.getPulses(); 
    rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);  
    counts_encoder2 = Encoder2.getPulses(); 
    rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);  
    pwm_motor2=0;
    pwm_motor1=0;
       }                 

// all lopes are closed, except  while (true)! 
//back to beginposition  by button !

    // motor 1  
    while(resetbutton==0 && rev_counts_motor1<-0.1 && value1_resetbutton >= 0){            
            direction_motor1 = ccw;                                    
            pwm_motor1 = 0.1;                             
          
             counts_encoder1 = Encoder1.getPulses();                 
             rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond); 
             value1_resetbutton = 1;                                    
            } //while loop closed
            
    while (resetbutton==0 && rev_counts_motor1>0.1 && value1_resetbutton <=0){ // werkt het zelfde als de vorige loop maar dan met tegengestelde richting.
    
             direction_motor1 = cw;
             pwm_motor1 = 0.1;
           
             counts_encoder1 = Encoder1.getPulses();
             rev_counts_motor1=counts_encoder1/(gearboxratio*rev_rond);
             
              value1_resetbutton = -1;
            } //while loop closed
            
    //motor 2       
    while(resetbutton==0 && rev_counts_motor2<-0.1 && value2_resetbutton >= 0){ // werkt het zelfde maar dan voor motor2
  
             direction_motor2 = cw;
             pwm_motor2 = 0.1;
             pwm_motor1 = 0;
          
             counts_encoder2 = Encoder2.getPulses();  
             rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
             value2_resetbutton = 1;    
            } //while loop closed
    
    while (resetbutton==0 && rev_counts_motor2>0.1 && value2_resetbutton <=0){
         
             direction_motor2 = ccw;
             pwm_motor2 = 0.1;
             pwm_motor1=0;
       
             counts_encoder2 = Encoder2.getPulses();
             rev_counts_motor2=counts_encoder2/(gearboxratio*rev_rond);
            
              value2_resetbutton = -1;
            }//while loop closed
pc.printf("rev count motor 1 is %f \r\n",rev_counts_motor1);
pc.printf("speed motor 1: %f\r\n", speed_motor2);
    }//while true closed
        
}//int main closed