File content as of revision 26:cd1db85359aa:

 //--------------------------------------------------------------------------------------------------------------------------//
 // 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(4);                      // 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);

// Tickers
    Ticker ScopeTime;
    Ticker myControllerTicker2;
    Ticker myControllerTicker1;
    Ticker myEMG1;
    Ticker myEMG2;

// Constants
    double reference2, reference1;
    double position2 = 0, position1 = 0;
    double m2_ref = 0, m1_ref = 0;
    int count = 0;
    double Grens2 = 90, Grens1 = 90;
    double Stapgrootte = 5;
    double Threshold;

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

//Controller gain Motor 2 & 1
    const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
    const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
    double m2_err_int = 0, m2_prev_err = 0;
    double m1_err_int = 0, m1_prev_err = 0;

//Derivative filter coeffs Motor 2 & 1
    const double BiGain2 = 0.012, BiGain1 = 0.016955;
    const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
    const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;

// Filter variables
    double m2_f_v1 = 0, m2_f_v2 = 0;
    double m1_f_v1 = 0, m1_f_v2 = 0;

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

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

// Biquad filter
    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;
    }    


// Reusable PID controller
    double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
        const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
    {
    // Derivative
        double e_der = (e-e_prev)/Ts;
        e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
        e_prev = e;
    // Integral
        e_int = e_int + Ts*e;
    // PID
        return Kp * e + Ki*e_int + Kd*e_der;
    }
    
//--------------------------------------------------------------------------------------------------------------------------//
//EMG functions
//--------------------------------------------------------------------------------------------------------------------------//

// Hier komen functies die de EMG signalen uitlezen en filteren

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

// Motor2 control
    void motor2_Controller() 
    {
        // Setpoint motor 2
            reference2 = m2_ref;                           // Reference in degrees
            position2 = Encoder2.getPulses()*360/(32*131); // 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, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
                m2_f_b0, m2_f_b1, m2_f_b2);
            double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
            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 = Encoder1.getPulses()*360/(32*131); // 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, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, 
                m1_f_b0, m1_f_b1, m1_f_b2); 
            double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2); 
            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, 0.01f ); // 100 Hz, Motor 2
        myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
        // 2 Tickers die de waarden van de EMG-signalen uitlezen
    
//--------------------------------------------------------------------------------------------------------------------------//
// Control Program
//--------------------------------------------------------------------------------------------------------------------------//
    while(true)
    {
        char c = pc.getc();
    // 1 Program UP
        if(c == 'e') // If ((EMG1 => Threshold) && (EMG2 => Threshold))
        {
            count = count + 1;
            if(count > 2)
                {
                    count = 2;
                }

        }
     // 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(c == 'r') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                {
                    m2_ref = m2_ref + Stapgrootte;
                    m1_ref = m1_ref - Stapgrootte;
                    if (m2_ref > Grens2)
                    {
                        m2_ref = Grens2;
                        m1_ref = -1*Grens1;
                    }
                }
                if(c == 'f') // if ((EMG2 => Threshold) && (EMG1 =< Threshold))
                {
                    m2_ref = m2_ref - Stapgrootte;
                    m1_ref = m1_ref + Stapgrootte;
                    if (m2_ref < -1*Grens2)
                    {
                        m2_ref = -1*Grens2;
                        m1_ref = Grens1;
                    }
                }
        }
    // PROGRAM 1: Motor 1 control, Red LED
        if(count == 1) 
        {
                LedG = LedB = 1;
                LedR = 0;
                if(c == 't') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                {
                    m1_ref = m1_ref + Stapgrootte;
                    if (m1_ref > Grens1)
                    {
                        m1_ref = Grens1;
                    }
                }
                if(c == 'g') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
                {
                    m1_ref = m1_ref - Stapgrootte;
                    if (m1_ref < -1*Grens1)
                    {
                        m1_ref = -1*Grens1;
                    }
                }
        }
    // PROGRAM 2: Firing mechanism & Reset, Blue LED
        if(count == 2) 
        {

                LedR = LedG = 1;
                LedB = 0;
                //VUUUUR!! (To Do)
                wait(1);
                m2_ref = 0;
                m1_ref = 0;
                count = 0;   
        }
    }

}