File content as of revision 1:757aadb68807:

#include "mbed.h"
#include "HIDScope.h"
#include "encoder.h"

//analoog in van potmeter 1
AnalogIn potmeter1(A0);

//signaal naar motor uit
DigitalOut motor1_direction(D7);// draairichting motor 1 (1 is CW )
PwmOut motor1_speed_control(D6);//aanstuursnelheid motor 1
//PwmOut motor2_speed_control(D5);
//DigitalOut motor2_direction(D4);

//encoders
Encoder encoder1(D13,D12);
//Encoder motor2_encoder(D11,D10);

//tickers
Ticker scopedataticker;
Ticker adjust_positionticker;





// Sample time (motor1−timestep)
const double m1_Ts = 0.01;

// Controller gains (motor1−Kp,−Ki,...)
const double m1_Kp = 2.5, m1_Ki = 1.0, m1_Kd = 0.5;
double m1_err_int = 0, m1_prev_err = 0;
// Derivative filter coefficients (motor1−filter−b0,−b1,...)
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;
 // Filter variables (motor1−filter−v1,−v2)
double m1_f_v1 = 0, m1_f_v2 = 0;

// Biquad filter (See slide 14)
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*2;
    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;
    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();
    double position = 0.002991*encoder1.getPosition(); // Don’t use magic numbers!
    double motor1 = PID( reference - position, 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 );
} // Attach this function to a Ticker



int main ()
{


}