File content as of revision 1:718c43124021:

#include "mbed.h"
#include "encoder.h"
#include "biquadFilter.h"

/*programma voor het testen van de beweging van de arm, aangestuurd door twee potmeters*/

//INPUT/OUTPUT
PwmOut pwm_motor1(D5); //onderste motor
DigitalOut dir_motor1(D4); 
PwmOut pwm_motor2(D6); //bovenste motor
DigitalOut dir_motor2(D7);
Encoder encoder1(D13,D12,true);
Encoder encoder2(D10,D9,true);
//potmeters voor testen:
AnalogIn potmeter1(A4);
AnalogIn potmeter2(A5);

//VARIABELEN
Ticker control_ticker;
double pos_1;
double pos_2;
double m1_f_v1 = 0, m1_f_v2 = 0;

// Sample time (motor1−timestep)
const double m1_Ts = 0.01;
// Controller gains (motor1−Kp,−Ki,...)
const double m1_Kp = 5, m1_Ki = 0.005, m1_Kd = 2;
double m1_err_int = 0, m1_prev_err = 0;
// Derivative filter coeicients (motor1−filter−b0,−b1,...)
const double m1_f_a1 = -0.11175688680, m1_f_a2 = 0.17749674417, m1_f_b0 = 1.0, m1_f_b1 = 2, m1_f_b2 = 1;


// Sample time (motor1−timestep)
const double m2_Ts = 0.01;
// Controller gains (motor1−Kp,−Ki,...)
const double m2_Kp = 5, m2_Ki = 0.005, m2_Kd = 2;
double m2_err_int = 0, m2_prev_err = 0;
// Derivative filter coeicients (motor1−filter−b0,−b1,...)
const double m2_f_a1 = -0.11175688680, m2_f_a2 = 0.17749674417, m2_f_b0 = 1.0, m2_f_b1 = 2, m2_f_b2 = 1;

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 with 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;
    /*biquadFilter derfilter(f_a1, f_a2, f_b0, f_b1, f_b2);
    e_der = derfilter.step(e_der);*/
    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 = (potmeter1.read()-0.5)*4200;
    double position = encoder1.getPosition(); // Don’t use magic numbers!
    double error = (reference - position)/1000;
    pos_1 = PID( error, 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 );
    if(pos_1 > 0)
        dir_motor1 = 0;
    else
        dir_motor1 = 1;

    pwm_motor1.write(abs(pos_1));

} // Attach this function to a Ticker

void m2_Controller()
{
    double reference = (potmeter2.read()-0.5)*4200;
    double position = encoder2.getPosition(); // Don’t use magic numbers!
    double error = (reference - position)/1000;
    pos_2 = PID( error, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, m2_f_a1, m2_f_a2, m2_f_b0, m2_f_b1, m2_f_b2 );
    if(pos_2 > 0)
        dir_motor2 = 1;
    else
        dir_motor2 = 0;

    pwm_motor2.write(abs(pos_2));

} // Attach this function to a Ticker

int main()
{
    pwm_motor1.period(0.0001);
    pwm_motor2.period(0.0001);
    control_ticker.attach(&m1_Controller, 0.01);
    control_ticker.attach(&m2_Controller, 0.01);

    while(1) {}  
}