File content as of revision 24:d0af4b2be295:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "biquadFilter.h"
 
Serial pc(USBTX, USBRX); // tx, rx
QEI Encoder2(D3, D2, NC, 32);
QEI Encoder1(D13,D12,NC, 32);
HIDScope scope(4);

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

//Motor
DigitalOut motor1direction(D6);
PwmOut motor1speed(D7);
DigitalOut motor2direction(D4);
PwmOut motor2speed(D5);

//Tickers
Ticker ScopeTime;
Ticker myControllerTicker2;

//Startwaarden
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;

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

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

//Derivative filter coeffs Motor 2
const double BiGain = 0.016955;
const double m2_f_a1 = -0.96608908283*BiGain, m2_f_a2 = 0.0*BiGain, m2_f_b0 = 1.0*BiGain, m2_f_b1 = 1.0*BiGain, m2_f_b2 = 0.0*BiGain;

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


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

// Motor2 control
void motor2_Controller() 
{
    reference2 = m2_ref; // Setpoint motor 2
    position2 = Encoder2.getPulses()*360/(32*131); // Aantal Degs motor 2
    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);
    motor2speed = abs(m2_P2); // Speed control
    if(m2_P2 > 0) // Direction control
    {  
        motor2direction = 0;
    }
    else
    {
        motor2direction = 1;
    }
    reference1 = m1_ref; // Setpoint
    position1 = Encoder1.getPulses()*360/(32*131); // Aantal Degs
    double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2, 
    m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilerd met dezelfde coeffs als motor 2
    double m1_P2 = biquad(m1_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); //Is gefilterd met dezelfde coeffs als motor 2
    motor2speed = abs(m1_P2); // Speed control
    if(m1_P2 > 0) // Direction control
    {  
        motor1direction = 0;
    }
    else
    {
        motor1direction = 1;
    }
}




int main()
{   
    LedR.write(1);
    LedB.write(1);
    LedG.write(1);
    pc.baud(115200);
    pc.printf("Tot aan loop werkt\n");
    
    ScopeTime.attach_us(&ScopeSend, 10e4);
    myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz

    while(true)
    {
        char c = pc.getc();
        if(c == 'e') //Ga 1 programma omhoog
        {
            count = count + 1;
            if(count > 2)
                {
                    count = 2;
                }

        }
        if(c == 'd') //Ga 1 programma omlaag
        {
            count = count - 1;
            if(count < 0)
                {
                    count = 0;
                }
        }      
        if(count == 0) //Motor 2 control, Groene LED
        {
                
                LedR = LedB = 1;
                LedG = 0;
                if(c == 'r')
                {
                    m2_ref = m2_ref + Stapgrootte;
                    if (m2_ref > Grens2)
                    {
                        m2_ref = Grens2;
                    }
                }
                if(c == 'f')
                {
                    m2_ref = m2_ref - Stapgrootte;
                    if (m2_ref < -1*Grens2)
                    {
                        m2_ref = -1*Grens2;
                    }
                }
        }
        if(count == 1) //Motor 1 control, Rode LED
        {
                LedG = LedB = 1;
                LedR = 0;
                if(c == 't')
                {
                    m1_ref = m1_ref + Stapgrootte;
                    if (m1_ref > Grens1)
                    {
                        m1_ref = Grens1;
                    }
                }
                if(c == 'g')
                {
                    m1_ref = m1_ref - Stapgrootte;
                    if (m1_ref < -1*Grens1)
                    {
                        m1_ref = -1*Grens1;
                    }
                }
        }
        if(count == 2) //Vuur mechanisme & Reset, Blauwe LED
        {

                LedR = LedG = 1;
                LedB = 0;
                //VUUUR!
                wait(1);
                m2_ref = 0;
                m1_ref = 0;   
        }
    }

}