File content as of revision 8:bc4b89961dd5:

#include "mbed.h"
#include "QEI.h"
#define SERIAL_BAUD 115200

/* IMPLEMENTED IN BIG FUNCTION, SO DO NOT CHANGE HERE BUT THERE!!!!!*/


//Initial allocations
Serial pc(USBTX,USBRX);

AnalogIn pot1(A1);
AnalogIn pot2(A2);

DigitalOut dirpin(D7); //Motor 1 (Rotatie)
PwmOut pwmpin(D6);

DigitalOut dirpin2(D4); //Motor 2 (Translatie)
PwmOut pwmpin2(D5);

QEI encoder1(D12,D13,NC,64,QEI::X4_ENCODING); //Encoder motor 1
QEI encoder2(D10,D11,NC,64,QEI::X4_ENCODING); // Encoder motor 2

const double Ts = 0.001;// sample frequency 

//constants motor
const double delta_t = 0.01;
const double L1 = 370.0 / 2.0;
const double L2 = 65.0/2.0;
const double pi = 3.14159265359;
const double alpha = (2.0 * pi) /(25.0*8400.0);
const double beta = (((2.0 * L1) - (2.0 * L2)) * 20.0 * pi) / (305.0 * 8400.0);
double rotation_end_position=1;
double tower_1_position=1;
double tower_end_position=1;
const double q1start = rotation_end_position * alpha;
const double q2start = tower_1_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!
const double q2end = tower_end_position * beta; //!!!!afhankelijk van een variabelle die in anderscript wordt bepaald!!!!

//variables motors
double out1;
double out2;
int counts1;
int counts2;
double vdesx;
double vdesy;
double q1;
double q2;
double MPe;
double xe;
double ye;
double gamma;
double dq1;
double dq2;
double dC1;
double dC2;
double pwm1;
double pwm2;

//PID rotation constants 
double Rot_Kp = 1.5;
double Rot_Ki = 0.1;
double Rot_Kd = 0.48;
double Rot_error = 0;
double Rot_prev_error = 0;

//PID translation constants
const double Trans_Kp = 0.5;
const double Trans_Ki = 0.5;
const double Trans_Kd = 0.1;
double Trans_error = 0;
double Trans_prev_error = 0;

// PID execution
double PID_control(double error, const double kp, const double ki, const double kd, double &error_int, double &error_prev)
{ 
// P control
double u_k = kp * error;

// I control
error_int = error_int + (Ts * error);
double u_i = ki * error_int;

// D control
double error_deriv = (error - error_prev);
double u_d = kd * error_deriv;
error_prev = error;

return u_k + u_i + u_d;
}

void boundaries(){
    double q2tot = q2 + dq2;
    if (q2tot > q2end){
        dq2 = 0;} //kan ook zeggen q2end-q2 is dat dan juiste waarde of moet q2-q2end?
    else if (q2tot < q2start){
        dq2 = 0;}
    else{}
    }

void motor_control()
{
    out1 = (pot1*2.0f)-1.0f; //control x-direction
    out2 = (pot2*2.0f)-1.0f; //control y-direction
    counts1 = encoder1.getPulses(); //counts encoder 1
    counts2 = encoder2.getPulses(); //counts encoder 2
    vdesx = out1 * 20.0; //speed x-direction
    vdesy = out2 * 20.0; //speed y-direction

    q1 = counts1 * alpha + q1start; //counts to rotation (rad) 
    q2 = counts2 * beta + q2start; //counts to translation (mm)
    MPe = L1 - L2 + q2; //x location end effector, x-axis along the translation
    xe = cos(q1) * MPe; //x location in frame 0
    ye = sin(q1) * MPe; //y location in frame 0
    gamma = 1.0 /((-1.0 * ye * sin(q1)) - (xe * cos(q1))); //(1 / det(J'')inverse)
    dq1 = gamma * delta_t * (sin(q1) * vdesx - cos(q1) * vdesy); //target rotation
    dq2 = gamma * delta_t * (-1.0 * xe * vdesx - ye * vdesy); //target translation
    boundaries();
    dC1 = PID_control( dq1, Rot_Kp, Rot_Ki, Rot_Kd, Rot_error, Rot_prev_error) / alpha; //target rotation to counts
    dC2 = PID_control( dq2, Trans_Kp, Trans_Ki, Trans_Kd, Trans_error, Trans_prev_error) / beta; //target translation to counts
    pwm1 = 3.0 * (dC1 / delta_t) / 8400.0; //
    pwm2 = 3.0 * (dC2 / delta_t) / 8400.0; //
    dirpin.write(pwm1 < 0);                    
    pwmpin = fabs (pwm1);                    
    dirpin2.write(pwm2 < 0);
    pwmpin2 = fabs (pwm2);
}


//main 
int main(){
    pc.baud(115200);                               
    pc.printf("start\r\n");
    pwmpin.period_us(60);
    
    while(1){
        motor_control();
        pc.printf("PWM_rot = %f PWM_trans = %f VdesX = %f VdesY = %f \n\r",pwm1,pwm2,vdesx,vdesy);
        wait(delta_t);
        }
}