File content as of revision 4:c2017b3a7adb:

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

#define     M_Pi 3.141592653589793238462643383279502884L

Serial      pc(USBTX, USBRX);

DigitalOut  led_red(LED_RED);
DigitalOut  led_blue(LED_BLUE);
InterruptIn button1(D2);
InterruptIn button2(D3);
AnalogIn    potmeter1(A0);
AnalogIn    potmeter2(A1);

DigitalOut  motor1DirectionPin(D4);
PwmOut      motor1MagnitudePin(D5);
DigitalOut  motor2DirectionPin(D7);
PwmOut      motor2MagnitudePin(D6);

Ticker      measureTick;

Encoder     motor1(D13,D12); //On the shield actually M2
Encoder     motor2(D11,D10); //On the shield actually M1 (Production mistake?)

bool         switch1 = 1; // manual switch for when to start calculations (later removed for a state machine
bool         direction1 = 1; // direction positive, 0 is negative
bool         direction2 = 1;
const double RAD_PER_PULSE = (2 * M_Pi)/8400 ; // Value for RAD_PER_PULSE given through the slides (wrong?)
const double DEG_PER_RAD = 180 / M_Pi; // Basic knowledge of how many degrees are in 1 radian.

double       q1 = 0;                // Angle of arm 1 (upper) in starting position is 0 degrees
double       q2 = 135/DEG_PER_RAD;  // Angle of arm 2 (lower) in starting position is 180 degrees (but can't be 0 or 180 because of determinant = 0)
int          L1 = 29;               // Length of arm 1 (upper) in cm
int          L2 = 47;               // Length of arm 2 (lower) in cm
//double       xdes = L1-L2;           // Desired x coordinate, arm is located at x = L1-L2 in starting position
//double       ydes = 0;              // Disired y coordinate, arm is located at y = 0 in starting position
double       MotorValue1 = 0;
double       MotorValue2 = 0;

// Sample time (motor1-timestep)
const double M1_Ts = 0.01; //100 Hz systems
const double M2_Ts = 0.01;

// Controller gains (motor1-Kp,-Ki,...)
const double M1_Kp = 0.1, M1_Ki = 0.0, M1_Kd = 0.00125,  M1_N = 100; // THESE VALUES ARE ARBITRARY AT THIS POINT Ki = 0.02
const double M2_Kp = 0.1, M2_Ki = 0.0, M2_Kd = 0.00125,  M2_N = 100; // Inspired by the Ziegler-Nichols Method

// Filter variables (motor1-filter-v1,-v2)
double M1_f_v1 = 0.0, M1_f_v2 = 0.0;
double M2_f_v1 = 0.0, M2_f_v2 = 0.0;

struct Q { double q1; double q2; };

//Kinematic model
void Kinematic_referencer(double &q1, double &q2, double a1, double a2, bool dir1, bool dir2)
    {
    double qt = q1 + q2, vx = 0, vy = 0;                       // current total angle
    double xcurrent = L1 * cos (q1) + L2 * cos (qt);   // current x position
    double ycurrent = L1 * sin (q1) + L2 * sin (qt);   // current y position
    
    //pc.printf("x = %.1f, y = %.1f \r\n", xcurrent, ycurrent);
    double velocity = 5;
    
    //Initial twist
    if (dir1 == 1){
        vx = velocity* !button1.read();}//(xdes - xcurrent)/0.05;  // Running on 100 Hz
    else {
        vx = -velocity* !button1.read();}
    if (dir2 == 1){
        vy = velocity* !button2.read();}//(ydes - ycurrent)/0.05;
    else {
        vy = -velocity* !button2.read();}
    
    //pc.printf("vx = %.2f, vy = %.2f \r\n", vx,vy);
    
    //Jacobians
    double J11 = -L1 * sin (q1) - L2 * sin (qt);
    double J12 = -L2 * sin (qt);
    double J21 = L1 * cos (q1) + L2 * cos (qt);
    double J22 = L2 * cos (qt);
    double Determinant = J11 * J22 - J21 * J12;   // calculate determinant
    
    //pc.printf("D = %.3f \r\n", Determinant);
 
    //Calculate angular velocities
    double q1der = ((J22 * vx) - (J12 * vy)) / Determinant;
    double q2der = ((-J21 * vx) + (J11 * vy)) / Determinant;
    
    //pc.printf("q1d = %.2f, q2d = %.2f \r\n", q1der, q2der);
    
    //Calculate new angles
    double q1new = q1 + (q1der * M1_Ts);    //nog fixen met die tijdstappen?
    double q2new = q2 + (q2der * M1_Ts);    //hier ook
    //pc.printf ("q1=%f, q2=%f\n", q1 * c, q2 * c);
    double qtnew = q1new + q2new;
    
    //Calculate new positions
    double xnew = L1 * cos (q1new) + L2 * cos (qtnew);
    double ynew = L1 * sin (q1new) + L2 * sin (qtnew);
    //printf ("x=%f, y=%f\n", x, y);
    
    q1 = q1new;
    q2 = q2new;
    
    /*
    // Now check whether the calculated position is desired, determinants close to zero may cause the robot to move weirdly
    // New y may not be negative, this means the arm is located in on the plate
    // New q1 may not be less than -45 degrees, less means the arm will crash into the base plate
    // New q2 may not be more than 185 degrees, more means the lower arm will crash into the upper arm
    if (ynew > -20 && a1new > -45 / DEG_PER_RAD && a2new < 200 / DEG_PER_RAD && (pow(xnew, 2.0) + pow(ynew,2.0)) > pow(17.0,2.0) )//&& Determinant < 0.01)
    {
        // If desired, change the angles
        q1 = a1new;
        q2 = a2new;
    }
    else
    {
        // If not desired, don't change the angles, but define current position as desired so the robot ignores the input
        xnew = xcurrent;
        ynew = ycurrent;
    }*/
}

// PROGRAM THAT CALCULATES THE PID
double PID( double err, const double Kp, const double Ki, const double Kd,
const double Ts, const double N, double &v1, double &v2 ) {

    const double a1 = -4/(N*Ts+2), a2 = -(N*Ts-2)/(N*Ts+2), // a1 and a2 are the nominator of our transferfunction
    b0 = (4*Kp + 4*Kd*N + 2*Ki*Ts + 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4),
    b1 = (Ki*N*pow(Ts,2) - 4*Kp - 4*Kd*N)/(N*Ts + 2), 
    b2 = (4*Kp + 4*Kd*N - 2*Ki*Ts - 2*Kp*N*Ts + Ki*N*pow(Ts,2))/(2*N*Ts + 4); // b0, b1 and b2 the denominator

    double v = err - a1*v1 - a2*v2; // Memory value are calculated and later on stored. (v is like an input)
    double u = b0*v + b1*v1 + b2*v2;
    v2 = v1; v1 = v;
    return u; // u functions as our output value gained from the transferfunction.
}

// PROGRAMS THAT CONTROLS THE VALUE OUTPUT
void M_Controller(double Angle1, double Angle2, double &MotorValue1, double &MotorValue2) {
    if (potmeter1 > 0.5f) {
        direction1 = 1;
        led_red = 0; }
    else {
        direction1 = 0;
        led_red = 1; }
        
    if (potmeter2 > 0.5f) {
        direction2 = 1;
        led_blue = 0; }
    else {
        direction2 = 0;
        led_blue = 1; }
    
    Kinematic_referencer(q1, q2, Angle1, Angle2, direction1, direction2);
        
    double ref_q1 = 2 * q1 * DEG_PER_RAD;
    double ref_q2 = q2 * DEG_PER_RAD;
    double a1 = DEG_PER_RAD * Angle1;
    double a2 = DEG_PER_RAD * Angle2;
    
    //pc.printf("a1 = %.2f, a2 = %.2f \r\n", a1, a2);
    //pc.printf("q1 = %.2f, q2 = %.2f \r\n", ref_q1, ref_q2);
    //pc.printf("e1 = %.2f, e2 = %.2f \r\n", ref_q1-a1, ref_q2-a2);
        
    MotorValue1 = PID( ref_q1 - a1 , M1_Kp, M1_Ki, M1_Kd, M1_Ts, M1_N, M1_f_v1, M1_f_v2); //Find the motorvalue by going through the PID
    MotorValue2 = PID( ref_q2 - a2 , M2_Kp, M2_Ki, M2_Kd, M2_Ts, M2_N, M2_f_v1, M2_f_v2);
}


// PROGRAMS FOR POWERING THE MOTOR ACCORDING TO THE ERROR (P VARIANT)
void SetMotor1(double motor1Value) // function that actually changes the output for the motor
{
    if(motor1Value >= 0 )  //Function sets direction and strength
        motor1DirectionPin = 1; //If the reference value is positive, we will turn clockwise
    else
        motor1DirectionPin = 0; // if not, counterclockwise
        
    if(fabs(motor1Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
        motor1MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
    else 
        motor1MagnitudePin = fabs(motor1Value); // if we fall underneath the safety, take the magnitude
}

void SetMotor2(double motor2Value) // function that actually changes the output for the motor
{
    if(motor2Value >= 0 )  //Function sets direction and strength
        motor2DirectionPin = 0; //If the reference value is positive, we will turn clockwise
    else
        motor2DirectionPin = 1; // if not, counterclockwise
        
    if(fabs(motor2Value) > 1.0 ) // Next, check the absolute motor value, which is the magnitude
        motor2MagnitudePin = 1.0; // This is a safety. We never want to exceed 1
    else 
        motor2MagnitudePin = fabs(motor2Value); // if we fall underneath the safety, take the magnitude
}

// PROGRAM THAT MEASURES AND CONTROLES THE MOTOR OUTPUT
void MeasureAndControl() // Pure values being calculated and send to the Mbed.
{     
    double Angle1 = RAD_PER_PULSE * motor1.getPosition(); // [rad]
    double Angle2 = RAD_PER_PULSE * motor2.getPosition() + 135 / DEG_PER_RAD; // [rad] 
    
    M_Controller(Angle1, Angle2, MotorValue1, MotorValue2 ); //Perhaps call the Motorvalues themselves inside this function and edit them that way...
    
    SetMotor1( MotorValue1 );
    SetMotor2( MotorValue2 );
}

int main() // Main function
{
    pc.baud(115200); // For post analysis, seeing if the plug works etc.
    pc.printf("STARTING SEQUENCE \r\n"); //Merely checking if there is a serial connection at all
    measureTick.attach(&MeasureAndControl, M1_Ts); // Tick that changes the motor (currently 1Hz)
    led_red = 1; // Set the LED off in the positive direction, on in the negative direction
    led_blue = 1;
}