File content as of revision 1:e5d2d638eaf4:

#include "stdio.h"
#include "math.h"
#include "mbed.h"
#include "QEI.h"
#include "MODSERIAL.h"

MODSERIAL pc(USBTX, USBRX);
QEI wheel_M1 (D10, D11, NC, 64);
QEI wheel_M2 (D13, D12, NC, 64);
PwmOut pwm_M1 (D5);
PwmOut pwm_M2 (D6);
DigitalOut dir_M1 (D4);
DigitalOut dir_M2 (D7);

Ticker PIDcontrol;

volatile double q1;
volatile double q2;
volatile double q1_prev = 0;
volatile double q2_prev = 0;
volatile double pwm_M1_ref = 0;
volatile double pwm_M2_ref = 0;
volatile double Pulses_M1;
volatile double Pulses_M2;

void initialize()
{
    dir_M1 = 0; //ccw
    dir_M2 = 1; //cw
    while (q1 > -20*2*3.1415/360) {
        Pulses_M1 = wheel_M1.getPulses();    //1334.355 counts/rad
        q1 = Pulses_M1 / (1334.355/3);     // rad
        pwm_M1_ref = 0.1;
    }
    pwm_M1_ref = 0;

    //while (q2 < 45*2*3.1415/360) {
    //    Pulses_M2 = wheel_M2.getPulses();
    //    q2 = Pulses_M2 / (1334.355/3);
    //    pwm_M2_ref = 0.1;
    //}
    //pwm_M2_ref = 0;

const double M1_TS = 0.002;
const double M1_Kp = 4.348, M1_Ki = 36.632, M1_Kd = 0.126;
double M1_err_int = 0, n1_prev_err = 0;
const double M1_F_A1 = 1.0, M1_F_A2 = 2.0, M1_F_B0 = 1.0, M1_F_B1 = 3.0, M1_F_B2 = 4.0; 
double M1_f_v1 = 0, M1_f_v2 = 0;

//Biquad filter

//volatile double u;
//volatile double v;
//volatile double v1;
//volatile double v2;
//volatile double y;
//double a1;
//double a2;
//double b0;
//double b1;
//double b2;

double biquad(double u, double &v1, double &v2, 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;
}

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

void M1_controller(){
    double reference = pwm_M1;
    q1 = wheel_M1.getPulses() / (1334.355/3);       // 1334.355/3 counts/rad
    double velocity = (q1 - q1_prev) / 0.002;
    q1_prev = q1;
    pwm_M1 = PID(reference - velocity, 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);
}

int main()
{
    pc.baud(115200);
    PIDcontrol.attach(&M1_controller, 0.002f);
    initialize();
}