File content as of revision 2:785737b1cd38:

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

// --------------------------------------------------
// ----------------- 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);

// 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.0;
double  point4x = 200.0;
double  point4y = 0.0;
const double x0 = 80.0; //zero x position after homing
const double y0 = 141.0; //zero y position after homing
double  setpointx = x0;
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;

// --------------------------------------------------------------------
// ---------------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 --------------------------------------
// -------------------------------------------------------------------------
void determinedemosetpoints( double &setpointx, double &setpointy)
{
    int track = 1;
    if( fabs(setpointx - point1x) <= 0.2 && fabs(setpointy - point1y) <= 0.2){
        track = 2;
        pc.printf("We gaan nu naar setpoint %i",track);
        }
    if( fabs(setpointx - point2x) <= 0.2 && fabs(setpointy - point2y) <= 0.2){
        track = 3;
        pc.printf("We gaan nu naar setpoint %i", track);
        }
    if( fabs(setpointx - point3x) <= 0.2 && fabs(setpointy - point3y) <= 0.2){
        track = 4;
        pc.printf("We gaan nu naar setpoint %i",track);
        }      
    
    switch(track){
        case 1:
        setpointx = setpointx + (point1x - x0)/150;
        setpointy = setpointy + (point1y - y0)/150;
        break;
        
        case 2:
        setpointx = setpointx + 0.2; 
        setpointy = setpointy;
        break;    
        
        case 3:
        setpointx = setpointx;
        setpointy = setpointy - 0.2;
        break;
        
        case 4:
        setpointx = setpointx - 0.2;
        setpointy = setpointy;
        break;
        }    
}
// -----------------------------------------------------------------    
// --------------------------- PI controllers ----------------------
// -----------------------------------------------------------------
double PI_controller1(double error1)
{
    static double error_integral1 = 0;
    
    // 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;
    
    // Sum 
    U1 = u_p1 + u_i1;
    
    // Return
    return U1;      
}
double PI_controller2(double error2)
{
    static double error_integral2 = 0;
    
    // 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;
    
    // Sum +
    U2 = u_p2 + u_i2;
    
    // 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))/(2.0*pi); //the actual amount of radians that the motor has to turn in total to reach the setpoint
    return -q2_diff;
}


// -----------------------------------------------
// ------------ RUN MOTORS -----------------------
// -----------------------------------------------
void motoraansturing()
{
    determinedemosetpoints(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); 
    
    while (true) {

    }
}