File content as of revision 3:1e0f4cb93eda:

#include "mbed.h"
#include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
#include "math.h"

float dist(float robot_x, float robot_y, float target_x, float target_y);

int main(){
    initRobot(); //Initializing the robot
    pc.baud(9600); // baud for the pc communication

    //Target example x,y values
    float target_x=46.8, target_y=78.6, target_angle=0;

    float a; //angle error
    float d; //distance from target
    float beta;
    //float k_linear=10, k_angular=200;
    float kr=3, ka=8, kb=-2;
    float linear, angular, angular_left, angular_right;
    float dt=0.5;
    float temp;

    //Diameter of a wheel and distance between the 2
    float r=3.25, b=7.2;

    int speed=999; // Max speed at beggining of movement

    //Resetting coordinates before moving
    theta=0; 
    X=0;
    Y=0;

    a = atan2((target_y-Y),(target_x-X))-theta;
    a = atan(sin(a)/cos(a));
    d = dist(X, Y, target_x, target_y);
    beta = -a-theta+target_angle;
    
    do {
        pc.printf("\n\n\r entered while");
        
        //Updating X,Y and theta with the odometry values
        Odometria();
        
        a = atan2((target_y-Y),(target_x-X))-theta;
        a = atan(sin(a)/cos(a));
        d = dist(X, Y, target_x, target_y);
        beta = -a-theta+target_angle;

        //Computing angle error and distance towards the target value
        d += dt*(-kr*cos(a)*d);
        a = temp;
        a += dt*(kr*sin(a)-ka*a-kb*b);
        b += dt*(-kr*sin(temp));
        pc.printf("\n\r d=%f", d);

        //Computing linear and angular velocities
        if(a>=-1.5708 && a<=1.5708){
            linear=kr*d;
            angular=ka*a+kb*beta;
        }
        else{
            linear=-kr*d;
            angular=-ka*a-kb*beta;
        }
        angular_left=(linear-0.5*b*angular)/r;
        angular_right=(linear+0.5*b*angular)/r;

        //Slowing down at the end for more precision
        if (d<25) {
            speed = d*30;
        }
        
        //Normalize speed for motors
        if(angular_left>angular_right) {
            angular_right=speed*angular_right/angular_left;
            angular_left=speed;
        } else {
            angular_left=speed*angular_left/angular_right;
            angular_right=speed;
        }

        pc.printf("\n\r X=%f", X);
        pc.printf("\n\r Y=%f", Y);

        //Updating motor velocities
        leftMotor(1,angular_left);
        rightMotor(1,angular_right);

        wait(dt);
    } while(d>1);

    //Stop at the end
    leftMotor(1,0);
    rightMotor(1,0);

    pc.printf("\n\r %f -- arrived!", d);
}

//Distance computation function
float dist(float robot_x, float robot_y, float target_x, float target_y){
    return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2));
}