File content as of revision 0:755bc7c0f555:

#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;
int track;
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;

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


    
    
 
        /*
    // Van punt 3 naar punt 4. 
    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
    {
        setpointx = setpointx - 0.1;          // Van punt 1 naar punt 2 op dezelfde y blijven. 
        track = 34;
    }
    if (setpointy > point3y && track == 34)
    {
        setpointx = setpointx - 0.1;
    }
        
    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
    {
        setpointx = 80.0;
    }
    */
      return setpointx;
}  

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

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

 /* 
    // Van punt 3 naar punt 4. 
    if (setpointy >= point3y - 0.3 && setpointx >= point3x - 0.3 && setpointy <= point3y + 0.3 && setpointx <= point3x + 0.3)
    {
        setpointy = setpointy;
        track = 34;
    }
    if (setpointy > point3y && track == 34)
    {
        setpointy = setpointy;
    }     
    
    
    if (setpointy >= point4y - 0.3 && setpointx >= point4x - 0.3 && setpointy <= point4y + 0.3 && setpointx <= point4x + 0.3)
    {
        setpointy = 141.0;
    }
    */ 
    return setpointy;
    
}

// -----------------------------------------------------------------    
// --------------------------- 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()
{
    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); 
    
    while (true) {

    }
}