File content as of revision 2:2c2c9d8238d4:

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





//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
MODSERIAL pc(USBTX, USBRX);
Ticker MyControllerTicker1;
Ticker MyControllerTicker2;
const double RAD_PER_PULSE = 0.00074799825*2;
const float CONTROLLER_TS = 0.01;
const double PI = 3.14159265358979323846;
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------




//------------------------------------------------------------------------------
//--------------------------------Servo-----------------------------------------
//------------------------------------------------------------------------------
Servo MyServo(D9);
InterruptIn But1(D8);
int k=0;

void servo_control (){
    if (k==0){
        MyServo = 0;
        k=1;
    }
    else{
        MyServo = 2;
        k=0;
        }
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





//------------------------------------------------------------------------------
//-------------------------------Kinematics-------------------------------------
//------------------------------------------------------------------------------
AnalogIn potmeter1(A3);
AnalogIn potmeter2(A4);
const float max_rangex = 450.0;
const float max_rangey = 300.0;
const float x_offset = 156.15;
const float y_offset = -76.97;
const float alpha_offset = -(21.52/180)*PI;
const float beta_offset  = 0.0;
const float L1 = 450.0;
const float L2 = 490.0;

float gettargetposition(double potmeter, int max_range){
    float target = potmeter * max_range;
    return target;
}

float getreferenceposition(float target, float offset) {
    float  referenceposition = target + offset;
    return referenceposition;
}

float getreferencealpha(float max_rangex, float maxrangey, float x_offset, float y_offset, float alpha_offset, float L1, float L2) {
    float x_target = gettargetposition(potmeter1, max_rangex);
    float y_target = gettargetposition(potmeter2, max_rangey);
    float x = getreferenceposition(x_target, x_offset);
    float y = getreferenceposition(y_target, y_offset);        
    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
    float alpha = alpha_ + alpha_offset;
    return alpha;
}
    
float getreferencebeta(float max_rangex, float maxrangey, float x_offset, float y_offset, float beta_offset, float L1, float L2) {
    float x_target = gettargetposition(potmeter1, max_rangex);
    float y_target = gettargetposition(potmeter2, max_rangey);
    float x = getreferenceposition(x_target, x_offset);
    float y = getreferenceposition(y_target, y_offset);    
    float alpha_ = (0.5*PI) - atan(y/x) - acos( ( (L1*L1) - (L2*L2) + (x*x) + (y*y) ) / (2*L1*sqrt( (x*x) +(y*y) ) ));
    float beta = acos( ((L1*L1) + (L2*L2) - (x*x) - (y*y)) / (2*L1*L2)) - alpha_ + beta_offset;
    return beta;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





//------------------------------------------------------------------------------
//------------------------------PI_Controller-----------------------------------
//------------------------------------------------------------------------------
float PI_controller(float error, const float Kp, const float Ki, float Ts, double &e_int) {
    e_int += Ts * error;
    return Kp * error + Ki * e_int ;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





//------------------------------------------------------------------------------
//--------------------------------Motor1----------------------------------------
//------------------------------------------------------------------------------
PwmOut motor1(D5);
DigitalOut motor1DirectionPin(D4);
DigitalIn ENC2A(D12);
DigitalIn ENC2B(D13);
Encoder encoder1(D13,D12);
const float MOTOR1_KP = 40.0;
const float MOTOR1_KI = 0.0;
double m1_err_int = 0;
const double motor1_gain = 2*PI;


void motor1_control(){
    float reference_alpha = getreferencebeta(max_rangex, max_rangey, x_offset, y_offset, beta_offset, L1, L2);
    float position_alpha = RAD_PER_PULSE * encoder1.getPosition();
    float error_alpha = reference_alpha-position_alpha;
    float magnitude1 = PI_controller(error_alpha, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int)/ (motor1_gain);
    motor1 = fabs(magnitude1);
    pc.printf("err_a = %f  alpha = %f   mag1 = %f\r\n", error_alpha, reference_alpha, magnitude1);
    pc.printf("\r\n");
    
    
    if (magnitude1  < 0){
        motor1DirectionPin = 1;
    }
    else{
        motor1DirectionPin = 0;
    }
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





//------------------------------------------------------------------------------
//--------------------------------Motor2----------------------------------------
//------------------------------------------------------------------------------
PwmOut motor2(D6);
DigitalOut motor2DirectionPin(D7);
DigitalIn ENC1A(D10);
DigitalIn ENC1B(D11);
Encoder encoder2(D10,D11);
const float MOTOR2_KP = 60.0;
const float MOTOR2_KI = 0.0;
double m2_err_int = 0;
const double motor2_gain = 2*PI;


void motor2_control(){
    float reference_beta = getreferencealpha(max_rangex, max_rangey, x_offset, y_offset, alpha_offset, L1, L2);
    float position_beta = RAD_PER_PULSE * -encoder2.getPosition();
    float error_beta = reference_beta-position_beta;
    float magnitude2 = PI_controller(error_beta, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int)/ (motor2_gain);
    motor2 = fabs(magnitude2);
    pc.printf("err2 = %f  beta = %f   mag2 = %f\r\n", error_beta, reference_beta, magnitude2);
    pc.printf("\r\n");
    
    if (magnitude2 > 0){
        motor2DirectionPin = 1;
    }
    else{
        motor2DirectionPin = 0;
    }
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------





int main(){
    pc.baud(115200);
    motor1.period(0.0002f);
    motor2.period(0.0002f);
    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
    But1.rise(&servo_control);
     
    
    
    
           
    while(1) {}   
}