File content as of revision 2:c2c76a7a7250:

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



DigitalOut gpo(D0);
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut motor1DC(D7);
DigitalOut motor1PWM(D6);
DigitalOut motor2DC(D4);
DigitalOut motor2PWM(D5);

AnalogIn   potMeter1(A0);
AnalogIn   potMeter2(A1);
DigitalIn  button1(D11);
DigitalIn  button2(D12);
QEI Encoder1(D12,D13,NC,4200);

MODSERIAL pc(USBTX,USBRX);

Ticker controller;

const double pi = 3.1415926535897;
const float M1_KP = 20 , M1_KI = 1.0, M1_KD = 0.5;
const double M1_TS = 0.001;
const float RAD_PER_PULSE = (2*pi)/4200;
const float CONTROLLER_TS = 0.001;     //TIME INTERVAL/ hZ
double m1_err_int = 0, m1_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
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;
}

/* the working P controller beneath here
//float P(double error, const float Kp){
//  return Kp * error;
//    }

// New PI controller

double PI(double e, const double Kp, const double Ki, double Ts, double &e_int){
    e_int += Ts*e;
    return Kp*e+Ki*e_int;
}*/

//new PID part

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 += Ts*e;
// PID
    return Kp * e + Ki * e_int + Kd * e_der;
}

/* Working P controller part

void motor1_Controller(){
    double reference = 10*potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    double motor1 = P(reference-position, M1_KP);
    motor1PWM = motor1;
    
    if(motor1 > 0.1){
        motor1DC = 1;
        
        ledr = 1;
        ledg = 1;       //Blau
        ledb = 0;
    }
    else if (motor1<-0.1) {
        motor1DC = 0; 
        
        ledb = 1;
        ledr = 1;
        ledg = 0;       //Groen
        
    }
    else{ 
    motor1PWM = 0;
        
        ledb = 1;       //Rood
        ledr = 0;
        ledg = 1;
    }
    
    
}
*/

/*/ New PI part 

void m1_Controller(){
    double reference = 10*potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
    
    motor1PWM = motor1;
    
    if(motor1 > 0.5){
        motor1DC = 1;
        
        ledr = 1;
        ledg = 1;       //Blau
        ledb = 0;
    }
    else if (motor1<-0.5) {
        motor1DC = 0; 
        
        ledb = 1;
        ledr = 1;
        ledg = 0;       //Groen
        
    }
    else{ 
    motor1PWM = 0;
        
        ledb = 1;       //Rood
        ledr = 0;
        ledg = 1;
    }
    
    
}
*/

void m1_Controller() {
    double reference = potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    //float 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 );
    //pc.printf("\r value motor1: %f. reference Pot: %f. Position: %f \n", motor1, reference, position);
    pc.printf("\r value motor1: . reference Pot: %f. Position: %f \n", reference, position);

    motor1PWM = motor1;

        if(motor1 > 0.5){
            motor1DC = 1;
        
            ledr = 1;
            ledg = 1;       //Blau
            ledb = 0;
        }
        else if (motor1<-0.5) {
            motor1DC = 0; 
        
            ledb = 1;
            ledr = 1;
            ledg = 0;       //Groen
        
        }
        else{ 
            motor1PWM = 0;
        
            ledb = 1;       //Rood
            ledr = 0;
            ledg = 1;
        }

}

int main(){
    pc.baud(115200);
    //controller.attach(&m1_Controller, CONTROLLER_TS);            --> P one
    
    controller.attach(&m1_Controller, M1_TS);
    
    while(1){    
    double reference = 10*potMeter1.read();
    double position = RAD_PER_PULSE*Encoder1.getPulses();
    // double motor1 = P(reference-position, M1_KP); --> old one
    //float motor1 = PI(reference - position, M1_KP, M1_KI, M1_TS, m1_err_int);
    float 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 );
             
    }
}