File content as of revision 6:5431ddb9d881:

#include "mbed.h"
#include "FastPWM.h"
#include "MODSERIAL.h"
#include "QEI.h"
#include "math.h"
//toch forken
// --------------------------------------------------
// ----------------- SET UP -------------------------
QEI Encoder1(D11, D10, NC, 4200) ;  // Encoder motor 1, (pin 1A, pin 1B, counts/rev)
QEI Encoder2(D9, D8, NC, 4200) ;    // Encoder motor 2, (pin 2A, pin 2B, counts/rev)
DigitalOut directionM1(D4);
DigitalOut directionM2(D7);
FastPWM motor1_pwm(D5);
FastPWM motor2_pwm(D6);
MODSERIAL  pc(USBTX, USBRX);
DigitalOut ledr(LED_RED);
DigitalOut ledg(LED_GREEN);
DigitalOut ledb(LED_BLUE);
AnalogIn  potmeter1(PTC10);
AnalogIn potmeter2(PTC11);


// Tickers
Ticker Demo;
Ticker Write;

// ---------------------------------------------------
// ----------------- GLOBAL VARIABLES ----------------
volatile int counts1; 
volatile int counts2;
volatile double theta1;
volatile double theta2;
const double  pi = 3.14159265359;
volatile double  error1;
volatile double  error2;
double  point1x = 200.0;
double  point1y = 200.0;
double  point2x = 350.0;
double  point2y = 200.0;
double  point3x = 350.0;
double  point3y = 0;
double  point4x = 200.0;
double  point4y = 0;
volatile int track = 1;
const double x0 = 80.0; //zero x position after homing
const double y0 = 141.0; //zero y position after homing
volatile double  setpointx = x0;
volatile double  setpointy = y0;
volatile double  U1;
volatile double  U2;

// Inverse Kinematics
volatile double q1_diff;
volatile double q2_diff;
double sq = 2.0; //to square numbers
const double L1 = 250.0; //length of the first link
const double L3 = 350.0; //length of the second link

// Reference angles of the starting position
double q2_0 = pi + acos((pow(x0,sq)+pow(y0,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3));
double q1_0 = atan(y0/x0)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x0,sq)+pow(y0,sq))/(2.0*L1*sqrt(pow(x0,sq)+pow(y0,sq))));
double q2_0_enc = q2_0 - q1_0;

// Controller
double potwaarde1;
double pot1;
double potwaarde2;
double pot2;
volatile double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
volatile double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
volatile double Kd1 = 1;                 // Kd (demping van de controller)
volatile double Kd2 = 1;                 // Kd (demping van de controller)


// --------------------------------------------------------------------
// ---------------Read out encoders------------------------------------
// --------------------------------------------------------------------    
double  counts2angle1()
{
    counts1 = Encoder1.getPulses();         // Counts of outputshaft of motor 1
    theta1 = -(double(counts1)/4200) * 2*pi;       // Angle of outputshaft of motor 1
    return theta1;
}

double counts2angle2()
{
    counts2 = Encoder2.getPulses();         // Counts of outputshaft of motor 2
    theta2 = (double(counts2)/4200) * 2*pi;       // Angle of outputshaft of motor 2
    return theta2;
}

// -------------------------------------------------------------------------
// -------------- Determine Setpoints --------------------------------------
// -------------------------------------------------------------------------
double determinedemosetx(double setpointx, double setpointy)
{

    if (setpointx < point1x && track == 1){ 
        setpointx = setpointx + 0.1;    
    }
    
    // Van punt 1 naar punt 2. 
    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && (track == 1 || track == 41)) {
        setpointx = setpointx + 0.1;
        track = 12;
    }
    if (setpointx < point2x && track == 12){
        setpointx = setpointx + 0.2;
    }
    
    // Van punt 2 naar punt 3. 
    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && track == 12)
    {
        setpointx = point3x; 
        track = 23;
    }
    
    if (setpointy > point3y && track == 23)
    {
        setpointx = point3x;          // Van punt 1 naar punt 2 op dezelfde x blijven. 
    } 
 

    // Van punt 3 naar punt 4. 
    if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
    {
        setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
        track = 34;
    }
    
    if (setpointx > point4x && track == 34)
    {
        setpointx = setpointx - 0.1;
    }
    
    // Van punt 4 naar punt 1.        
    if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
    {
        setpointx = point4x;
        track = 41;
    }
    
    if (setpointy < point1y && track == 41)
    {
        setpointx = point4x;        // Van punt 4 naar punt 2 op dezelfde x blijven.
    }
    

      return setpointx;
}  

double determinedemosety(double setpointx, double setpointy)
{
    // Van reference positie naar punt 1.
    if(setpointy < point1y && track == 1){
        setpointy = setpointy + (0.2);
    } 

    // Van punt 1 naar punt 2. 
    if (fabs(setpointx - point1x) <= 0.3 && fabs(setpointy - point1y) <= 0.3 && (track == 1 || track == 41)){
        ledg = 1; ledr = 0; ledb = 1;   // Turns the Led RED
        setpointy = point2y;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
        track = 12;
    }
    if (setpointx < point2x && track == 12){
        setpointy = point2y;
    }
    
    // Van punt 2 naar punt 3. 
    if (fabs(setpointx - point2x) <= 0.3 && fabs(setpointy - point2y) <= 0.3 && (track == 12)){
        ledr = 1; ledg = 0; ledb = 1;   // Turns the Led GREEN
        setpointx = point3x;
        track = 23;
    }
    if ((setpointy > point3y) && (track == 23))
    {
        setpointy = setpointy + (-0.2);
        track = 23;
    }    
    
    // Van punt 3 naar punt 4. 
    if ((fabs(setpointx - point3x) <= 0.3) && (fabs(setpointy - point3y) <= 0.3) && (track == 23))
    {
        ledg = 1; ledr = 1; ledb = 0;   // Turns the Led BLUE
        setpointy = setpointy;
        track = 34;
    }
    if (setpointx > point4x && track == 34)
    {
        setpointy = setpointy;
    }     
    
    // Van punt 4 naar punt 1.  
    if ((fabs(setpointx - point4x) <= 0.3) && (fabs(setpointy - point4y) <= 0.3) && (track == 34))
    {
        ledg = 0; ledr = 0; ledb = 0;   // Turns the LED WHITE
        track = 41;
    }
    
    if (setpointy < point1y && track == 41)
    {
        setpointy = setpointy + (0.2);        // Van punt 4 naar punt 2 op dezelfde x blijven.
    }

    return setpointy;
    
}

// -----------------------------------------------------------------    
// --------------------------- PI controllers ----------------------
// -----------------------------------------------------------------
double PI_controller1(double error1)
{
    static double error_integral1 = 0;
    static double error_prev1 = error1; // initialization with this value only done once!
    
    // Proportional part
    //double Kp1 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p1 = Kp1*error1;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki1 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts1 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral1 = error_integral1 + error1 * Ts1;  
    double u_i1 = Ki1 * error_integral1;
    
    // Derivative part
    double error_derivative1 = (error1 - error_prev1)/Ts1; 
    double u_d1 = Kd1 * error_derivative1; 
    error_prev1 = error1;
    
    // Sum 
    U1 = u_p1 + u_i1 + u_d1;
    
    // Return
    return U1;      
}
double PI_controller2(double error2)
{
    static double error_integral2 = 0;
    static double error_prev2 = error2; // initialization with this value only done once!
    
    // Proportional part
    //double Kp2 = 3.95;              // Kp (proportionele controller, nu nog een random waarde)
    double u_p2 = Kp2*error2;        // Voltage dat naar de motor gestuurd wordt (volgt uit error en Kp)
    
    // Integral part
    double Ki2 = 6.0;             // Ki (Integrale deel vd controller, nu nog een random waarde)
    double Ts2 = 0.005;           // Sample tijd, net zo vaak als de controller wordt aangeroepen (200 Hz, statemachine)   
    error_integral2 = error_integral2 + error2 * Ts2;  
    double u_i2 = Ki2 * error_integral2;
    
    // Derivative part
    double error_derivative2 = (error2 - error_prev2)/Ts2; 
    double u_d2 = Kd2 * error_derivative2; 
    error_prev2 = error2;
    
    // Sum +
    U2 = u_p2 + u_i2 + u_d2;
    
    // Return
    return U2;      
} 
// ------------------------------------------------------------
// ------------ Inverse Kinematics ----------------------------
// ------------------------------------------------------------
double makeAngleq1(double x, double y){
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    q1_diff = -2.0*(q1-q1_0);                                                                                    //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return q1_diff;
}

double makeAngleq2(double x, double y){
    double q2 = -acos((pow(x,sq)+pow(y,sq)-pow(L1,sq)-pow(L3,sq))/(2.0*L1*L3)); //angle of the second joint in setpoint configuration
    double q1 = atan(y/x)+acos((-pow(L3,sq)+pow(L1,sq)+pow(x,sq)+pow(y,sq))/(2.0*L1*sqrt(pow(x,sq)+pow(y,sq)))); //angle of the first joint in the setpoint configuration
    double q2_motor = (pi - q2) - q1; //because q2 represents the angle at joint two and not at the motor a calculation has to be done
    q2_diff = (2.0*(q2_motor - q2_0_enc)); //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return q2_diff;
}


// -----------------------------------------------
// ------------ RUN MOTORS -----------------------
// -----------------------------------------------
void motoraansturing()
{
    setpointx = determinedemosetx(setpointx, setpointy);
    setpointy = determinedemosety(setpointx, setpointy);
    q1_diff = makeAngleq1(setpointx, setpointy);
    q2_diff = makeAngleq2(setpointx, setpointy);

    theta2 = counts2angle2();           
    error2 = q2_diff - theta2;
    theta1 = counts2angle1();  
    error1 = q1_diff - theta1;          // Setpoint error, te behalen setpoint minus de huidige positie van de as.

    U1 = PI_controller1(error1);       // Voltage dat naar de motor gestuurd wordt. 
    U2 = PI_controller2(error2);   
    
    motor1_pwm.write(fabs(U1));         // Motor aansturen
    directionM1 = U1 > 0.0f;            // Richting van de motor bepalen
    motor2_pwm.write(fabs(U2));
    directionM2 = U2 > 0.0f; 
}


void rundemo()
{
    motoraansturing();
}


int main()
{
    pc.baud(115200);
    motor1_pwm.period_us(60);                // Period is 60 microseconde
    motor2_pwm.period_us(60);
    Demo.attach(&rundemo, 0.005f); 
    
    pc.printf("\r\n_____ DEMO _____\r\n\r\n");
    
    ledr = 1;
    ledg = 1;
    ledb = 1;
    
    while (true) {
        //pc.printf("Setpointx: %0.2f, Setpointy: %02f, q1_diff: %0.2f, q2_diff: %0.2f, error1: %0.2f, error2: %0.2f, U1: %0.2f, U2: %0.2f\r\n", setpointx,setpointy,q1_diff,q2_diff,error1,error2,U1,U2);
        pc.printf("Kp-waarde: %0.3f en Kd-waarde: %0.3f\r\n",Kp1,Kd1);
        if (track == 1) {
            pc.printf("Gaat van home naar positie 1\r\n");
        }
        else if (track == 12) {
            pc.printf("Gaat naar positie 2\r\n");
        }
        else if (track == 23) {
            pc.printf("Gaat naar positie 3\r\n");
        }
        else if (track == 34) {
            pc.printf("Gaat naar positie 4\r\n");
        }
        else if (track == 41) {
            pc.printf("Gaat naar positie 1\r\n");
        }
        
        
        potwaarde1 = potmeter1.read();  // Lees de potwaardes uit
        pot1 = potwaarde1*1.1;
        if (pot1 <= 0.10) 
        {
            Kp1 = 3.95;
            Kp2 = 3.95;
        } 
        else if (pot1 >= 0.10)
        {
            Kp1 = 3.95 + 40*(pot1-0.1);
            Kp2 = 3.95 + 40*(pot1-0.1);
        }
        potwaarde2 = potmeter2.read();  // Lees de potwaardes uit
        pot2 = potwaarde2*1.1;
        
        if (pot2 <= 0.10)
        {
            Kd1 = 0;
            Kd2 = 0;
        }
        else if (pot2 >= 0.10)
        {
            Kd1 = 10*(pot2-0.1);
            Kd2 = 10*(pot2-0.1);
        }
        
        wait(0.5f);
    }
}