File content as of revision 6:6ae6256cf234:

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

MODSERIAL pc(USBTX, USBRX);

Ticker MyControllerTicker1;
Ticker MyControllerTicker2;
const double PI = 3.141592653589793;
const double RAD_PER_PULSE = 0.00074799825;
const double CONTROLLER_TS = 0.01; //0.01


//Motor1
PwmOut motor1(D5);
DigitalOut motor1DirectionPin(D4);
DigitalIn ENC2A(D12);
DigitalIn ENC2B(D13);
Encoder encoder1(D13,D12);
AnalogIn potmeter1(A3);
const double MOTOR1_KP = 20;
const double MOTOR1_KI = 10;
double m1_err_int = 0;
const double motor1_gain = 2*PI;


//Motor2
PwmOut motor2(D6);
DigitalOut motor2DirectionPin(D7);
DigitalIn ENC1A(D10);
DigitalIn ENC1B(D11);
Encoder encoder2(D10,D11);
AnalogIn potmeter2(A4);
const double MOTOR2_KP = 20;
const double MOTOR2_KI = 10;
double m2_err_int = 0;
const double motor2_gain = 2*PI;

//________________________________________________________________
//Kinematica

//Motor offsets (kinematica implementation)
int max_rangex = 400;
int max_rangey = 250;
int L1 = 450;
int L2 = 490;
float alphaoffset = 0.577872;
float betaoffset = -0.165529+0.577872; //21.52;
float x_offset = 0.0;//165.07;
float y_offset = 0.0;//106.37;


float getreferencepositionx(double potmeter){
    float x_target = potmeter * max_rangex;
    return x_target;
}
float getreferencepositiony(double potmeter){
    float y_target = potmeter * max_rangey;
    return y_target;
}

float getreferenceanglealpha(const double PI, float x_offset, float y_offset, float alphaoffset){
    float x = getreferencepositionx(potmeter1) + x_offset;
    float y = getreferencepositiony(potmeter2) - y_offset;
    float theta2 = acos( (x*x + y*y - L1*L1 - L2*L2) / (2*L1*L2) );
    float theta1 = asin( (L2*sin(theta2)) / sqrt(x*x +y*y)) + atan(y/x);
    float alpha = (0.5*PI) - theta1 - alphaoffset;
    return alpha;
}

float getreferenceanglebeta(const double PI, float x_offset, float y_offset, float betaoffset, float alphaoffset){
    float x = getreferencepositionx(potmeter1) + x_offset;
    float y = getreferencepositiony(potmeter2) - y_offset;
    float theta2 = acos((x*x + y*y - L1*L1 - L2*L2)/(2*L1*L2));
    float theta1 = asin( (L2*sin(theta2)) / sqrt(x*x +y*y)) + atan(y/x);
    float alpha = (0.5*PI) - theta1 - alphaoffset;
    float beta = PI - alpha - theta2 - betaoffset;
    return beta;
}     

//________________________________________________________________


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

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

void motor1_control(){
    double referenceangle1 = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset);
    double position1 = RAD_PER_PULSE * encoder1.getPosition();
    double magnitude1 = PI_controller(referenceangle1-position1, MOTOR1_KP, MOTOR1_KI, CONTROLLER_TS, m1_err_int) / motor1_gain;
    motor1 = fabs(magnitude1);
    
    if (magnitude1 < 0){
        motor1DirectionPin = 1;
    }
    else{
        motor1DirectionPin = 0;
    }
}


void motor2_control(){
    double referenceangle2 = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset);
    double position2 = RAD_PER_PULSE * encoder2.getPosition();
    double magnitude2 = PI_controller(referenceangle2-position2, MOTOR2_KP, MOTOR2_KI, CONTROLLER_TS, m2_err_int) / motor2_gain;
    motor2 = fabs(magnitude2);
    
    if (magnitude2 < 0){
        motor2DirectionPin = 1;
    }
    else{
        motor2DirectionPin = 0;
    }
}

/*void servo_control (){
    motor1.period(0.02f);
    motor2.period(0.02f);
    if (k==0){
        MyServo = 0;
        k=1;
        motor1.period_us(200);
    }
    else{
        MyServo = 2;
        k=0;
        motor1.period_us(200);
    }        
}*/


int main(){
    
    pc.baud(115200);
    pc.printf("Hello World!\r\n");

    
    //But1.rise(&servo_control);
    motor1.period(0.0002f);
    motor2.period(0.0002f);
    MyControllerTicker1.attach(&motor1_control, CONTROLLER_TS); 
    MyControllerTicker2.attach(&motor2_control, CONTROLLER_TS);
    
    
    const double PI = 3.141592653589793; 
    float x_offset = 165.07;
    float y_offset = 106.37; 
    float alphaoffset = 0.577872;
    float betaoffset = -0.165529+0.577872; //21.52;
    float alpha = getreferenceanglealpha(PI, x_offset, y_offset, alphaoffset);
    pc.printf("a %f\n", alpha);
 
    float beta = getreferenceanglebeta(PI, x_offset, y_offset, betaoffset, alphaoffset);
    pc.printf("b %f\n", beta);
       
    while(1) {}   
}