File content as of revision 12:95d57057d622:

 //--------------------------------------------------------------------------------------------------------------------------//
 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
 //--------------------------------------------------------------------------------------------------------------------------//
 // Libraries
 //--------------------------------------------------------------------------------------------------------------------------//
#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "biquadFilter.h"

//--------------------------------------------------------------------------------------------------------------------------//
// Constanten/Inputs/Outputs
//--------------------------------------------------------------------------------------------------------------------------//
    MODSERIAL pc(USBTX, USBRX);             // To/From PC
    QEI Encoder2(D3, D2, NC, 32);           // Encoder Motor 2
    QEI Encoder1(D13,D12,NC, 32);           // Encoder Motor 1
    HIDScope scope(6);                      // Scope, 4 channels

// LEDs
    DigitalOut LedR(LED_RED);
    DigitalOut LedG(LED_GREEN);
    DigitalOut LedB(LED_BLUE);

// Motor
    DigitalOut motor1direction(D7);         // Motor 1, Direction & Speed
    PwmOut motor1speed(D6);
    DigitalOut motor2direction(D4);         // Motor 2, Direction & Speed
    PwmOut motor2speed(D5);

//EMG
    AnalogIn    EMG_left(A0);               //Analog input
    AnalogIn    EMG_right(A1);   

// Tickers
    Ticker      ScopeTime;
    Ticker      myControllerTicker2;
    Ticker      myControllerTicker1;
    Ticker      SampleEMGLeft;
    Ticker      SampleEMGRight;
    Ticker      ScopeTimer;
    Ticker      serial;
    Ticker      MovingAverageLeft;
    Ticker      MovingAverageRight;
    
// Constants
    double reference2, reference1;
    double position2 = 0, position1 = 0;
    double m2_ref = 0, m1_ref = 0;
    int count = 0;
    double Grens2 = 60, Grens1 = 60;
    double Stapgrootte = 1;
   
// Declaring variables
    double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
    double EMG_L_fh=0;
    double EMG_left_value;
    double EMG_left_f1;
    double EMG_left_f2;
    double EMG_left_f3;
    double EMG_left_abs;

    double EMG_right_value;
    double EMG_right_f1;
    double EMG_right_f2;
    double EMG_right_f3;
    double EMG_right_abs;
    double Threshold1;
    double Threshold3;
    
    int N = 50;
    double MAF_left[50];
    double EMG_left_MAF;
    double MAF_right[50];
    double EMG_right_MAF;
    

//Sample time (motor-step)
    const double m2_Ts = 0.001, m1_Ts = 0.001;

//Controller gain Motor 2 & 1
    const double m2_Kp = 0.02686,m2_Ki = 0.01854, m2_Kd = 0.001172;
    const double m1_Kp = 0.02686,m1_Ki = 0.01854, m1_Kd = 0.001172;
    double m2_err_int = 0, m2_prev_err = 0;
    double m1_err_int = 0, m1_prev_err = 0;
    
// coëfficiënten
    const double BiGainEMG_H1 = 0.796821; 
    const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
    
    const double BiGainEMG_L1= 0.001041;
    const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
   
    const double BiGainEMG_N1 = 1.0;
    const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
    
// Creating the filters
    biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
    biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter 
    biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter
    
    biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2);        // creates the high pass filter
    biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2);         // creates the low pass filter  
    biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2);           // creates the notch filter

    
//--------------------------------------------------------------------------------------------------------------------------//
// General Functions
//--------------------------------------------------------------------------------------------------------------------------//

//Blinking led
void BlinkLed(DigitalOut LED, int n, float Time)
{ 
    for(int i=1; i<=n;i++)
    {
        LED.write(0);
        wait(Time);
        LED.write(1);
        wait(Time);     
    }
    LED.write(1);
}

//HIDScope
    void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
    {
        scope.set(0, position2);  
        scope.set(1, position1);
        scope.set(2, EMG_left_MAF);
        scope.set(3, EMG_right_MAF);
        scope.set(4, count);
        scope.send();
    
    }

// Reusable PID controller
    double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
    {
    // Derivative
        double e_der = (e-e_prev)/Ts;
        e_prev = e;
    // Integral
        e_int = e_int + Ts*e;
    // PID
        return Kp * e + Ki*e_int + Kd*e_der;
    }
    
//--------------------------------------------------------------------------------------------------------------------------//
//EMG functions
//--------------------------------------------------------------------------------------------------------------------------//

// EMG filtering function
void EMGfilterLeft()
{
    EMG_left_value = EMG_left.read();
    EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
    EMG_left_abs = fabs(EMG_left_f1);
    EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs);
    EMG_left_f3 = EMG_notch1.step(EMG_left_f2);

}

void EMGfilterRight()
{
    EMG_right_value = EMG_right.read();
    EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
    EMG_right_abs = fabs(EMG_right_f1);
    EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs);
    EMG_right_f3 = EMG_notch1R.step(EMG_right_f2);
}

// Movingaverage Filter
  void MovingAverageFilterLeft()
    {
        EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N;
        MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25];
        MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0];
        MAF_left[0] = EMG_left_f3;
    }
    
    void MovingAverageFilterRight()
    {
        EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N;
        MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25];
        MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0];
        MAF_right[0] = EMG_right_f3;
    }

//--------------------------------------------------------------------------------------------------------------------------//
// Motor control functions
//--------------------------------------------------------------------------------------------------------------------------//

// Motor2 control
    void motor2_Controller() 
    {
        // Setpoint motor 2
            reference2 = m2_ref;                           // Reference in degrees
            position2 = 360.0*Encoder2.getPulses()/(32.0*131.0); // Position in degrees
        // Speed control
            double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
            double m2_P2 = m2_P1;
            motor2speed = abs(m2_P2); 
        // Direction control
            if(m2_P2 > 0) 
            {    
                motor2direction = 0;
            }
            else
            {
                motor2direction = 1;
            }
    }   

// Motor1 control
    void motor1_Controller() 
    {
        // Setpoint Motor 1
            reference1 = m1_ref;                           // Reference in degrees
            position1 = 360.0*Encoder1.getPulses()/(32.0*131.0); // Position in degrees
        // Speed control
            double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
            double m1_P2 = m1_P1;
            motor1speed = abs(m1_P2); 
        // Direction control    
            if(m1_P2 > 0)
            {  
                motor1direction = 1;
            }
            else
            {
                motor1direction = 0;
            }
    }

//--------------------------------------------------------------------------------------------------------------------------//
// Main function
//--------------------------------------------------------------------------------------------------------------------------//
int main()
{  
//--------------------------------------------------------------------------------------------------------------------------//
// Initalizing
//--------------------------------------------------------------------------------------------------------------------------// 
    //LEDs OFF
        LedR = LedB = LedG = 1;
    
    //PC connection & check
        pc.baud(115200);
        pc.printf("Tot aan loop werkt\n");
    
    // Tickers
        ScopeTime.attach(&ScopeSend, 0.01f);                    // 100 Hz, Scope
        myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 100 Hz, Motor 2
        myControllerTicker1.attach(&motor1_Controller, m1_Ts ); // 100 Hz, Motor 1
        SampleEMGLeft.attach(&EMGfilterLeft, 0.01f);
        SampleEMGRight.attach(&EMGfilterRight, 0.01f);
        MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f);
        MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f);
//--------------------------------------------------------------------------------------------------------------------------//
// Determing Threshold 
//--------------------------------------------------------------------------------------------------------------------------//      
        wait(20);
    
        BlinkLed(LedG, 2, 0.2);             //bepaling threshold linkerarm
        LedG.write(0);
        wait(2);
        Threshold1 = 0.5*EMG_left_MAF;
        LedG.write(1);
        
        BlinkLed(LedR, 1, 2);         

        BlinkLed(LedB, 2, 0.2);             //bepaling threshold rechterarm
        LedB.write(0);
        wait(2);
        Threshold3 = 0.5*EMG_right_MAF;
        LedB.write(1);
                
        pc.printf("Threshold links = %f, Threshold rechts = %f/n", Threshold1, Threshold3); 
        BlinkLed(LedR, 1, 2);
        LedR.write(1);    
//--------------------------------------------------------------------------------------------------------------------------//
// Control Program
//--------------------------------------k------------------------------------------------------------------------------------//
    while(true)
    {
                                            //char c = pc.getc();
    // 1 Program UP
       if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
        {
            count = count + 1;
            if(count > 2)
                {
                    count = 2;
                }
            wait(1);
        }
     // 1 Program DOWN
     //   if(c == 'd') // Hoe gaat dit aangestuurd worden?
     //   {
     //       count = count - 1;
     //       if(count < 0)
     //           {
     //               count = 0;
     //           }
     //   }
    // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED      
        if(count == 0)
        {
                LedR = LedB = 1;
                LedG = 0;
                
                if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) //if(c == 'r') //
                {
                    m2_ref = m2_ref + Stapgrootte;
                    m1_ref = m1_ref + Stapgrootte;
                    if (m2_ref > Grens2)
                    {
                        m2_ref = Grens2;
                        m1_ref = Grens1;
                    }
                    wait(0.05);
                }
                if((EMG_right_MAF < Threshold3) && (EMG_left_MAF > Threshold1)) //if (c == 'f') //   
                {
                    m2_ref = m2_ref - Stapgrootte;
                    m1_ref = m1_ref - Stapgrootte;
                    if (m2_ref < -1*Grens2)
                    {
                        m2_ref = -1*Grens2;
                        m1_ref = -1*Grens1;
                    }
                    wait(0.05);
                }
        }
    // PROGRAM 1: Motor 1 control, Red LED
        if(count == 1) 
        {
                LedG = LedB = 1;
                LedR = 0;
                //Linker grenzen
                if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref < 0)) // if(c == 't') // Draait naar links
                {
                    m1_ref = m1_ref + Stapgrootte;
                    
                    if (m1_ref > m2_ref + 35 )
                    {
                        m1_ref = m2_ref + 35 ;
                    }
                    wait(0.05);
                }
                if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref < 0)) //if(c == 'g') // Draait naar rechts
                {
                    m1_ref = m1_ref - Stapgrootte;
                  
                    if (m1_ref < -1*Grens1 )
                    {
                        m1_ref = -1*Grens1;
                    }
                    wait(0.05);
                }
                //Rechter grenzen
                if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref >= 0)) // if(c == 't') // Draait naar links
                {   
                    m1_ref = m1_ref + Stapgrootte;
                    
                    if (m1_ref > Grens1 )
                    {
                        m1_ref = Grens1;
                    }
                    wait(0.05);
                }
                if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref >= 0)) //if(c == 'g') // Draait naar rechts
                {   
                    m1_ref = m1_ref - Stapgrootte;
                  
                    if (m1_ref < m2_ref - 35 )
                    {
                        m1_ref = m2_ref - 35 ;
                    }
                    wait(0.05);
                }
        }
    // PROGRAM 2: Firing mechanism & Reset, Blue LED
        if(count == 2) 
        {
                LedG = LedR = 1;
                pc.printf("Please press the button to shoot the disc/n");
                BlinkLed(LedB, 25, 0.1);
                
                //VUUUUR!! (To Do)
                while (m2_ref != 0)                 // positiearm terug naar startpositie
                {
                if(m2_ref < 0)
                {
                    m2_ref = m2_ref+Stapgrootte;
                    wait(0.05);
                }
                if(m2_ref > 0)
                {
                    m2_ref = m2_ref-Stapgrootte;
                    wait(0.05);
                }
                }
                
                while (m1_ref != 0)                     //werparm terug naar startpositie
                {
                if(m1_ref<0)
                {
                    m1_ref = m1_ref+Stapgrootte;
                    wait(0.05);
                }
                
                if(m1_ref>0)
                {    
                    m1_ref = m1_ref-Stapgrootte;
                    wait(0.05);
                }
                }
                    
                if((m2_ref == 0) && (m1_ref == 0))              // terug naar program 0
                {
                    count = 0;   
                }
        }

}
}