File content as of revision 18:622e717da184:

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


//Reference signals
DigitalOut gpo(D0);
DigitalOut ledb(LED_BLUE);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);

//Motors
DigitalOut motor1DC(D7);
DigitalOut motor2DC(D4);
FastPWM motor1PWM(D6);
FastPWM motor2PWM(D5);

//Position control
AnalogIn    potMeter1(A0);
AnalogIn    potMeter2(A1);
AnalogIn    emg1(A2);   
AnalogIn    emg2(A3);
AnalogIn    emg3(A4);
AnalogIn    emg4(A5);
Encoder     Encoder1(D12,D13);
Encoder     Encoder2(D8,D9);

//callibrating
DigitalIn   button1(SW2);   //(x,y) = (..,..)
DigitalIn   button2(SW3);   //(x,y) = (..,..)

MODSERIAL pc(USBTX,USBRX);
Ticker controller;

// --- EMG ---
double in1;
double in2;
double RA;

double in3;
double in4;
double LA;

// Constants Position ----------------------- Start
double X;
double X_maxTreshold = 450;
double X_minTreshold = 20;
const double X_Callibrate  = 300;

double Y;
double Y_maxTreshold = 450;
double Y_minTreshold = 0;
const double Y_Callibrate  = 300;


// ---- Motor Constants-------
double motor1;
double motor2;
int SetPosM1;
int SetPosM2;

const double pi = 3.1415926535897;
const double   Pwmperiod = 0.001f;
const double   gainM1 = 1/29.17;       // 1 / encoder pulses per degree theta
const double   gainM2 = 1/105.0;       // 1 / pulses per mm

double reference_motor1 = 0;     // reference for Theta
double reference_motor2 = 0;
double pos_M1 = 0;               // start angle theta
double pos_M2 = 0;               // start radius


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

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 = 30; 
const float     M2_KI = 0.5;
const float     M2_KD = 0.5; //was KP=10 KI=0.5 KD=0.5, KP increased for faster response
double          m2_err_int = 0;
double          m2_prev_err = 0;

// Constants Biquad
BiQuad bq1(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
BiQuad bq2(6.38946e-01,-1.27789e+00,6.38946e-01,-1.14298e+00,4.12802e-01);
BiQuadChain bqc;
//---------------------------------End of constants PID

//-----------------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 e_der1 = (e1 - e_prev1)/Ts; // Ts = motor1-timestep                 // Derivative
    
    e_der1 = bqc.step(e_der1);   
    e_prev1 = e1;
    e_int1 += Ts*e1;
    
    if(button1 == 0 || button2 ==0){
        e1 = 0;
        e_prev1 = 0;
        e_int1 = 0;
        e_der1 = 0;
    }
                                                              // Integral   
    return Kp1*e1 + Ki1*e_int1 + Kd1 * e_der1;                                    //PID
}

double PID2(double e2, const double Kp2, const double Ki2, const double Kd2, double Ts, double &e_int2, double &e_prev2){

    double e_der2 = (e2 - e_prev2)/Ts; // Ts = motor1-timestep                 // Derivative
    // biquad part, see slide
    //e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
    
    e_der2 = bqc.step(e_der2);
    
    e_prev2 = e2;
    e_int2 += Ts*e2;
    
    if(button1 == 0 || button2 ==0){
        e2 = 0;
        e_prev2 = 0;
        e_int2 = 0;
        e_der2 = 0;
    }
                                                              // Integral   
    return Kp2*e2 + Ki2*e_int2 + Kd2 * e_der2;                                    //PID
}

// --- Get reference position ---
float Get_X_Position(){
   
// --- Altering X value with potmeter ---
    if (potMeter1 < 0.3){
                X = X-0.5;
        }
        else if (potMeter1> 0.7){
                X = X+0.5;
        }
        else{
                X = X;
    }
    
// --- Getting reference values from emg input ---
    in1 = emg1.read();
    in2 = emg2.read();
    RA = in1+in2;
    
    
    if (RA < 0.8){
                X = X;
        }
        else if (RA > 1.8){
                X = X-0.4;
        }
        else{
                X = X+0.4;
    }

// --- Setting max and min boundaries for X ---
    if (X >= X_maxTreshold){
        X = X_maxTreshold;
        }
        else if (X <= X_minTreshold){
            X = X_minTreshold;
        }
        else{
            X = X;
    } 
            
 // --- Setting callibration values for X ---
    if(button1 == 0){
        X = X_minTreshold;
        }
        else if (button2 == 0){
            X = X_Callibrate;
            }
        else{
            X = X;
            }
    //pc.baud(115200);
    //pc.printf("\r (in1,in2):(%f,%f), RA = %f, X = %f \n",in1, in2, RA, X);
    return X;
}

double Get_Y_Position(){
// --- Altering Y value with potmeter --- 
    if (potMeter2 < 0.3){
                Y = Y-0.8;
            }
        else if (potMeter2 > 0.7){
                Y = Y+0.5;
        }
        else{
                Y = Y;
    }
// --- Getting reference values from emg input ---
    in3 = emg3.read();
    in4 = emg4.read();
    LA = in3+in4;
    
    if (LA < 0.8){                   
                Y = Y;
                
                ledb = 1;                   // Implement LED feedback to give feedback to the user whether Y is increasing, decreasing, or stationairy.
                ledr = 1;                   // The same is done for X on the transmitting embed, this way there is feedback for both X and Y.
                ledg = 0;       //Groen
            }
        else if (LA > 1.8){ 
                Y = Y-0.4;
                
                ledr = 1;
                ledg = 1;       //Blau
                ledb = 0;
        }
        else{
                Y = Y+0.4;
                
                ledb = 1;       //Rood
                ledr = 0;
                ledg = 1;
    }
// --- Setting max and min boundaries for Y ---           
    if (Y >= Y_maxTreshold){
        Y = Y_maxTreshold;
        }
        else if (Y <= Y_minTreshold){
            Y = Y_minTreshold;
        }
        else{
            Y = Y;
    } 
 // --- Setting callibration values for Y ---            
    if(button1 == 0){
        Y = Y_minTreshold;
        }
        else if (button2 == 0){
            Y = Y_Callibrate;
        }
        else{
            Y = Y;
    }        
    //pc.printf("\r (in3,in4):(%f,%f), LA = %f, Y = %f \n",in3, in4, LA, Y);
    return Y;
}
//----------------------------------------------------END

//-------------Get current Position-------------------START
double motor1_Position(){ // has as output Theta
// --- Setting callibration values for the encoder ---    
    if (button1 == 0){
           SetPosM1 = ((atan(Y_minTreshold/X_minTreshold)*180)/pi)/gainM1;
           Encoder1.setPosition(SetPosM1);
        }
        else if (button2 ==0){
            SetPosM1 = ((atan(Y_Callibrate/X_Callibrate)*180)/pi)/gainM1;
            Encoder1.setPosition(SetPosM1);
        }
        else{          
        }
// --- getting current position ----      
    double pos_M1 = gainM1*Encoder1.getPosition(); // current position for theta
    double countM1 = Encoder1.getPosition();
    
    pc.baud(115200);
    //pc.printf("\r counts encoder: %f\n",countM1);
    return pos_M1;       
}
double motor2_Position(){ //output R
// --- Setting callibration values for the encoder ---      
    if (button1 == 0){
           SetPosM2 = (sqrt(X_minTreshold*X_minTreshold+Y_minTreshold*Y_minTreshold))/gainM2;
           Encoder2.setPosition(SetPosM2);
        }
        else if (button2 ==0){
            SetPosM2 = (sqrt(X_Callibrate*X_Callibrate+Y_Callibrate*Y_Callibrate))/gainM2;
            Encoder2.setPosition(SetPosM2);
        }
        else{          
        }
// --- getting current position ----        
    double pos_M2 = gainM2*Encoder2.getPosition(); // current position for the radius;
    double countM2 = Encoder2.getPosition();
    pc.baud(115200);
    //pc.printf("\r counts encoder: %f\n",countM2);
    return pos_M2;
}
//-----------------------------------------------------END


//------------Controller-------------------------------START
void Controller(){
    
    double x = Get_X_Position();
    double y = Get_Y_Position();
     
    reference_motor1 = (atan(y/x)*180)/pi;       // reference for Theta
    reference_motor2 = sqrt((x*x+y*y));          // reference for radius
    
    pos_M1 = motor1_Position(); // current position for theta   
    pos_M2 = motor2_Position(); // current position for the radius
      
    double deltaM1 = PID1(reference_motor1 - pos_M1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err);
    double deltaM2 = PID2(reference_motor2 - pos_M2, M2_KP, M2_KI, M2_KD, M1_TS, m2_err_int, m2_prev_err);

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

    pc.baud(115200);
    //pc.printf("\r DesPosition(X,Y):(%f,%f), posError(dTheta, dError):(%f,%f), (deltaM1,deltaM2):(%f,%f)\n",x,y, dTheta ,dRadius,deltaM1, deltaM2);
    pc.printf("\r DesPosition(X,Y):(%f,%f)\n", x,y);
    //pc.printf("\r pos(M1,M2):(%f,%f)\n", pos_M1, pos_M2);
    
// --- Direction control motors --- START
    if(deltaM1 > 0.5){ 
        motor1DC = 0; 
    }
    else if (deltaM1< -0.5) {
        motor1DC = 1; 
    }
    else{ 
    motor1PWM = 0;
    }

if(deltaM2 > 2.0){
        motor2DC = 0;
    }
    else if (deltaM2<-2.0) {
        motor2DC = 1;        
    }
    else{ 
    motor2PWM = 0;
    }
// --- Direction control motors --- END

// --- PWM values --- 
 motor1 = abs(deltaM1)/1000.0;
        if(motor1 >= 0.10) {
        motor1 = 0.10;
      //pc.baud(115200);
      //pc.printf("\r val motor1: %f\n", motor1);
    }

motor2 = abs(deltaM2)/1000.0;
    if(motor2 >= 0.50) {
        motor2 = 0.50;
    }
    
    motor1PWM = motor1 + 0.90;
    motor2PWM = motor2 + 0.50;
    
    //pc.printf("\r delta(1,2):(%f,%f)\n", deltaM1,deltaM2);
    //pc.printf("\r motorvalues (M1,M2):(%f,%f),\n",  motor1 + 0.65, motor2 + 0.20);
    //pc.printf("\r 
}

int main()
{
    controller.attach(&Controller, M1_TS);
    motor1PWM.period(Pwmperiod);
    motor2PWM.period(Pwmperiod);
    
    ledr = 1;
    ledb = 1;
    ledg = 1;
    
    bqc.add(&bq1).add(&bq2);
    
    while(1){}    
    
}