File content as of revision 4:5f7d1654108d:

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



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

AnalogIn   potMeter1(A0);
AnalogIn   potMeter2(A1);
DigitalIn  button1(D11);
DigitalIn  button2(D12);
Encoder Encoder1(D12,D13);
Encoder Encoder2(D8,D9);

MODSERIAL pc(USBTX,USBRX);

Ticker controller;

// ---- Motor Constants-------
float Pwmperiod = 0.0001f;
int potmultiplier = 800;    // Multiplier for the pot meter reference which is normally between 0 and 1
float gainM1 = 1/35.17;     // encoder pulses per degree theta
float gainM2 = 0.01;      // gain for radius r

// new PID constants, will have to be determined trough trial and error.

double kp = 250;
double ki = 100;
double kd = 0;


volatile float motor1;
volatile float motor2;

//Start constants PID ------------------------------- 
const double pi = 3.1415926535897;
const double M1_TS = 0.001; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)

//verplaatst
const float     RAD_PER_PULSE = (2*pi)/4200;
const float     CONTROLLER_TS = 0.01;     //TIME INTERVAL/ hZ

//----PID constants
const float     M1_KP = 10;
const float     M1_KI = 0.5;
const float     M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
double          m1_err_int = 0; 
double          m1_prev_err = 0;

const float     M2_KP = 10;
const float     M2_KI = 0.5;
const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
double          m2_err_int = 0; 
double          m2_prev_err = 0;

//---- Biquad constants---------
const double    M1_F_A1  = 1.0 ;
const double    M1_F_A2  = 2.0; 
const double    M1_F_B0  = 1.0; 
const double    M1_F_B1  = 3.0; 
const double    M1_F_B2  = 4.0;
double          m1_f_v1  = 0; 
double          m1_f_v2  = 0;


//-----------------Start PID part----------------------------START
double PID1(double e1, const double Kp1, const double Ki1, const double Kd1, double Ts, double &e_int1, double &e_prev1, 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){

    double e_der1 = (e1 - e_prev1)/Ts;         // Derivative, Ts = motor1-timestep
    // biquad part, see slide
    //e_der1 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
    e_prev1 = e1;
    e_int1 += Ts*e1;                          // Integral   
       
    return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1;
       
}

double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2, 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){

    double e_der2 = (e2 - e_prev2)/Ts;         // Derivative, Ts = motor1-timestep
    // biquad part, see slide
    //e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
    e_prev2 = e2;
    e_int2 += Ts*e2;                          // Integral   
    
    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;

}

//------------Get reference position-----------------START
float Get_X_Position(){
    double X = potMeter1 * potmultiplier;
    return X;
    
}

float Get_Y_Position(){
    double Y = potMeter2 * potmultiplier;
    return Y;
}
//----------------------------------------------------END

//-------------Get current Position-------------------START
double motor1_Position(){ // has as output Theta
    double pos_m1 = gainM1*Encoder1.getPosition(); // current position for theta      
    return pos_m1;       
}
double motor2_Position(){ //output R
    double pos_m2 = gainM2 *Encoder2.getPosition(); // current position for the radius;
    pc.baud(115200);
    pc.printf("\r x = %f",pos_m2); 
    
    return pos_m2;
}
//-----------------------------------------------------END


//------------Controller-------------------------------START
void Controller(){
    
    double x = Get_X_Position();
    double y = Get_Y_Position();
     
    double reference_motor1 = (atan(y/x)*180)/pi;       // reference for Theta
    double reference_motor2 = sqrt((x*x+y*y));          // reference for radius
    
    float pos_M1 = motor1_Position(); // current position for theta   
    float pos_M2 = motor2_Position(); // current position for the radius
    
    double delta1 = PID1(reference_motor1 - pos_M1, 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);
    double delta2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);

    double dTheta   = reference_motor1 - pos_M1;
    double dRadius  = reference_motor2 - pos_M2;
    
    

    pc.baud(115200);
    //pc.printf("\r DesPosition(X,Y):(%f,%f), pos Error(dTheta, dError):(%f,%f)\n",x,y, dTheta ,dRadius);
    //pc.printf("\r PID1 result: %f\n", delta1);
    
    //motor1PWM = motor1;
    //motor2PWM = motor2;

    if(delta1 > 10.0){
        motor1DC = 0;
        
        ledr = 1;
        ledg = 1;       //Blau
        ledb = 0;
    }
    else if (delta1< -10.0) {
        motor1DC = 1; 
        
        ledb = 1;
        ledr = 1;
        ledg = 0;       //Groen
        
    }
    else{ 
    motor1PWM = 0;
        
        ledb = 1;       //Rood
        ledr = 0;
        ledg = 1;
    }
    
    motor1 = abs(delta1)/1000.0f;
    if(motor1 >= 0.50f) {
        motor1 = 0.50f;
    }

if(delta2 > 10.0){
        motor2DC = 0;
        
        ledr = 1;
        ledg = 1;       //Blau
        ledb = 0;
    }
    else if (delta2<-10.0) {
        motor2DC = 1; 
        
        ledb = 1;
        ledr = 1;
        ledg = 0;       //Groen
        
    }
    else{ 
    motor2PWM = 0;
        
        ledb = 1;       //Rood
        ledr = 0;
        ledg = 1;
    }
    
    motor2 = abs(delta2)/1000.0f;
    if(motor1 >= 0.50f) {
        motor1 = 0.50f;
    }
    
    motor1PWM = motor1 + 0.50f;
    motor2PWM = motor1 + 0.50f;
    
    //pc.printf("\r motorvalues (M1,M2):(%f,%f), error:( \n", , motor1PWM, motor2PWM);
    //pc.printf("\r 
}

int main()
{
    controller.attach(&Controller, M1_TS);
    //motor1PWM.period(Pwmperiod);
    //motor2PWM.period(Pwmperiod);
    
    while(1){
    /*
    double x = Get_X_Position();
    double y = Get_Y_Position(); 
    double reference_motor1 = atan(y/x);  
    int position_Motor1 = motor1_Position();  
    double motor1 = PID(reference_motor1 - position_Motor1, 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.baud(115200);
    pc.printf("\r Position(X)=(%f), Ref(Theta,R): (%f,), Pos(Theta,R):(%i,), Motor Value(M1,M2):(%f,).\n",x, reference_motor1, position_Motor1, motor1);    
    */   
    }    
    
}