File content as of revision 25:572c9e9a8809:

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

 using namespace std;

//fill initialLogValues with the values we already know (here the bordurs)
void fill_initial_log_values();
//generate a position randomly and makes the robot go there while updating the map
void randomize_and_map();
//go the the given position while updating the map
void go_to_point_with_angle(float target_x, float target_y, float target_angle);
//Updates sonar values
void update_sonar_values(float leftMm, float frontMm, float rightMm);
//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected);
//print the map
void print_final_map();
//print the map with the robot marked on it
void print_final_map_with_robot_position();

//MATHS heavy functions
float dist(float robot_x, float robot_y, float target_x, float target_y);
//returns the probability [0,1] that the cell is occupied from the log value lt
float log_to_proba(float lt);
//returns the log value that the cell is occupied from the probability value [0,1]
float proba_to_log(float p);
//returns the new log value
float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue );
//makes the angle inAngle between 0 and 2pi
float rad_angle_check(float inAngle);
//returns the angle between the vectors (x,y) and (xs,ys)
float compute_angle_between_vectors(float x, float y,float xs,float ys);
float robot_center_x_in_orthonormal_x();
float robot_center_y_in_orthonormal_y();
float robot_sonar_front_x_in_orthonormal_x();
float robot_sonar_front_y_in_orthonormal_y();
float robot_sonar_right_x_in_orthonormal_x();
float robot_sonar_right_y_in_orthonormal_y();
float robot_sonar_left_x_in_orthonormal_x();
float robot_sonar_left_y_in_orthonormal_y();
float estimated_width_indice_in_orthonormal_x(int i);
float estimated_height_indice_in_orthonormal_y(int j);

const float pi=3.14159;
//spec of the sonar
//TODO MEASURE THE DISTANCE on X and Y of the robot frame, between each sonar and the center of the robot and add it to calculus in updateSonarValues
const float RANGE_SONAR=50;//cm
const float RANGE_SONAR_MIN=10;//Rmin cm
const float INCERTITUDE_SONAR=10;//cm
const float ANGLE_SONAR=pi/3;//Omega rad
const float SECURITY_DISTANCE=150;//mm

//those distance and angle are approximation in need of measurements
const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees
const float DISTANCE_SONAR_LEFT_X=4;
const float DISTANCE_SONAR_LEFT_Y=4;

const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees
const float DISTANCE_SONAR_RIGHT_X=-4;
const float DISTANCE_SONAR_RIGHT_Y=4;

const float ANGLE_FRONT_TO_FRONT=0;
const float DISTANCE_SONAR_FRONT_X=0;
const float DISTANCE_SONAR_FRONT_Y=5;

//TODO adjust the size of the map for computation time (25*25?)
const float WIDTH_ARENA=120;//cm
const float HEIGHT_ARENA=90;//cm

//const int SIZE_MAP=25;
const int NB_CELL_WIDTH=24;
const int NB_CELL_HEIGHT=18;

//position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
const float DEFAULT_X=WIDTH_ARENA/2;
const float DEFAULT_Y=HEIGHT_ARENA/2;
const float DEFAULT_THETA=pi/2;

//used to create the map 250 represent the 250cm of the square where the robot is tested
//float sizeCell=250/(float)SIZE_MAP;
float sizeCellX=WIDTH_ARENA/(float)NB_CELL_WIDTH;
float sizeCellY=HEIGHT_ARENA/(float)NB_CELL_HEIGHT;

float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell
float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT];

//Diameter of a wheel and distance between the 2
const float RADIUS_WHEELS=3.25;
const float DISTANCE_WHEELS=7.2;

const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP

bool tooClose=false;
bool turnFromObstacle=false;

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

    measure_always_on();//TODO check if needed
    wait(0.5);
    
    fill_initial_log_values();

    theta=DEFAULT_THETA; 
    X=DEFAULT_X;
    Y=DEFAULT_Y;

    for (int i = 0; i<20; i++) {
        randomize_and_map();
        wait(2);
        print_final_map();    
    }

}

//fill initialLogValues with the values we already know (here the bordurs)
void fill_initial_log_values(){
     //Fill map, we know the border are occupied
    for (int i = 0; i<NB_CELL_WIDTH; i++) {
        for (int j = 0; j<NB_CELL_HEIGHT; j++) {
            if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1)
                initialLogValues[i][j] = proba_to_log(1);
            else
                initialLogValues[i][j] = proba_to_log(0.5);
        }
    }
}

//generate a position randomly and makes the robot go there while updating the map
void randomize_and_map() {
    //TODO check that it's aurelien's work
    float target_x = (rand()%(int)(WIDTH_ARENA*10))/10;//for decimal precision
    float target_y = (rand()%(int)(HEIGHT_ARENA*10))/10;
    float target_angle = ((float)(rand()%31416)-15708)/10000.0;
    
    //TODO comment that
    //pc.printf("\n\r targ_X=%f", target_x);
    //pc.printf("\n\r targ_Y=%f", target_y);
    //pc.printf("\n\r targ_Angle=%f", target_angle);
    
    go_to_point_with_angle(target_x, target_y, target_angle);
}

//go the the given position while updating the map
//TODO clean this procedure it's ugly as hell and too long
void go_to_point_with_angle(float target_x, float target_y, float target_angle) {
    if(tooClose){
        target_x=X;
        target_y=Y;
        target_angle=theta+pi/2;
        target_angle = atan(sin(target_angle)/cos(target_angle));
        pc.printf("\n\rShould just turn now, new target_angle=%f\n\r", target_angle);
    }

    //TODO ugly old stuff
    float alpha; //angle error
    float rho; //distance from target
    float beta;
    float kRho=12, ka=30, kb=-13; //Kappa values
    float linear, angular, angular_left, angular_right;
    float dt=0.5;
    float temp;
    float d2;
    Timer t;

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

    float leftMm;
    float frontMm;
    float rightMm; 
        
    alpha = atan2((target_y-Y),(target_x-X))-theta;
    alpha = atan(sin(alpha)/cos(alpha));
    rho = dist(X, Y, target_x, target_y);
    
    beta = -alpha-theta+target_angle;
    //beta = atan(sin(beta)/cos(beta));

    do {
        //pc.printf("\n\n\r entered while");
        
        //Timer stuff
        dt = t.read();
        t.reset();
        t.start();
        
        //Updating X,Y and theta with the odometry values
        Odometria();

        leftMm = get_distance_left_sensor();
        frontMm = get_distance_front_sensor();
        rightMm = get_distance_right_sensor();

        //pc.printf("\n\r leftMm=%f", leftMm);
        //pc.printf("\n\r frontMm=%f", frontMm);
        //pc.printf("\n\r rightMm=%f", rightMm);

        update_sonar_values(leftMm, frontMm, rightMm);

        if ((leftMm < SECURITY_DISTANCE || frontMm < SECURITY_DISTANCE || rightMm < SECURITY_DISTANCE) && turnFromObstacle==false) {
            tooClose = true;
            turnFromObstacle = true;
            pc.printf("\n\r TOO CLOSE \n\r");
            leftMotor(1,0);
            rightMotor(1,0);
            Odometria();
            go_to_point_with_angle(X, Y, rad_angle_check(theta+pi));
            break;    
        }
        
        alpha = atan2((target_y-Y),(target_x-X))-theta;
        alpha = atan(sin(alpha)/cos(alpha));
        rho = dist(X, Y, target_x, target_y);
        d2 = rho;
        beta = -alpha-theta+target_angle;        
        
        //Computing angle error and distance towards the target value
        rho += dt*(-kRho*cos(alpha)*rho);
        temp = alpha;
        alpha += dt*(kRho*sin(alpha)-ka*alpha-kb*beta);
        beta += dt*(-kRho*sin(temp));
        //pc.printf("\n\r d2=%f", d2);
        //pc.printf("\n\r dt=%f", dt);

        //Computing linear and angular velocities
        if(alpha>=-1.5708 && alpha<=1.5708){
            linear=kRho*rho;
            angular=ka*alpha+kb*beta;
        }
        else{
            linear=-kRho*rho;
            angular=-ka*alpha-kb*beta;
        }
        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
        
        pc.printf("\n\r a_r=%f", angular_right);
        pc.printf("\n\r a_l=%f", angular_left);

        //Slowing down at the end for more precision
        //if (d2<25) {
//            speed = d2*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);

        pc.printf("\n\r a_r=%f", angular_right);
        pc.printf("\n\r a_l=%f", angular_left);

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

        wait(0.2);
        //Timer stuff
        t.stop();
    } while(d2>1 && (abs(target_angle-theta)>0.01) && tooClose==false);

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

    if(turnFromObstacle){
        turnFromObstacle=false;
        tooClose=false;
    }
}

//Updates sonar values
void update_sonar_values(float leftMm, float frontMm, float rightMm){

    float currProba;
    float i_in_orthonormal;
    float j_in_orthonormal;
    for(int i=0;i<NB_CELL_WIDTH;i++){
        for(int j=0;j<NB_CELL_HEIGHT;j++){
                //check if the point A(x,y) in the world frame is within the range of the sonar (which has the coordinates xs, ys in the world frame)
            //check that again
            //compute for front sonar
            i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i);
            j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j);

            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_front_x_in_orthonormal_x(),robot_sonar_front_y_in_orthonormal_y(),ANGLE_FRONT_TO_FRONT,frontMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];//map is filled as map[0][0] get the data for the point closest to the origin
            //compute for right sonar
            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_right_x_in_orthonormal_x(),robot_sonar_right_y_in_orthonormal_y(),ANGLE_FRONT_TO_RIGHT,rightMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
             //compute for left sonar
            currProba=compute_probability_t(i_in_orthonormal,j_in_orthonormal,robot_sonar_left_x_in_orthonormal_x(),robot_sonar_left_y_in_orthonormal_y(),ANGLE_FRONT_TO_LEFT,leftMm/10);
            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
        }
    }
}

//ODOMETRIA MUST HAVE BEEN CALLED
//function that check if a cell A(x,y) is in the range of the front sonar S(xs,ys) (with an angle depending on the sonar used, front 0, left pi/3, right -pi/3) returns the probability it's occupied/empty [0;1]
float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){

    float alpha=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam
    float alphaBeforeAdjustment=alpha-theta-angleFromSonarPosition;
    alpha=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure
    float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2));

    //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS
    //check if absolute difference between the angles is no more than Omega/2
    if( distancePointToSonar < (RANGE_SONAR)&& (alpha <= ANGLE_SONAR/2 || alpha >= rad_angle_check(-ANGLE_SONAR/2))){
        if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){
        //point before obstacle, probably empty
        /*****************************************************************************/
            float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
            float Er;
            if(distancePointToSonar < RANGE_SONAR_MIN){
                //point before minimum sonar range
                Er=0.f;
            }else{
                //point after minimum sonar range
                Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2);
            }
         /*****************************************************************************/
            return (1.f-Er*Ea)/2.f;
        }else{
            //probably occupied
        /*****************************************************************************/
            float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2);
            float Or;
            if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){
                //point between distanceObstacleDetected +- INCERTITUDE_SONAR
                Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2);
            }else{
                //point after in range of the sonar but after the zone detected
                Or=0;
            }
        /*****************************************************************************/
            return (1+Or*Oa)/2;
        }
    }else{
        //not checked by the sonar
        return 0.5;
    }
}

void print_final_map() {
    float currProba;
    pc.printf("\n\r");
    for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
        for (int x= 0; x<NB_CELL_WIDTH; x++) {
                currProba=log_to_proba(map[x][y]);
            if ( currProba < 0.5) {
                pc.printf(" 0 ");
            } else {
                if(currProba==0.5)
                    pc.printf(" . ");
                else
                    pc.printf(" + ");
            }
        }
        pc.printf("\n\r");
    }
}

void print_final_map_with_robot_position() {
    float currProba;
    Odometria();
    float Xrobot=robot_center_x_in_orthonormal_x();
    float Yrobot=robot_center_y_in_orthonormal_y();
    
    float heightIndiceInOrthonormal;
    float widthIndiceInOrthonormal;
    
    float heightMalus=-(3*sizeCellX/2);
    float heightBonus=sizeCellX/2;
    
    float widthMalus=-(3*sizeCellY/2);
    float widthBonus=sizeCellY/2;

    pc.printf("\n\r");
    for (int y = NB_CELL_HEIGHT -1; y>-1; y--) {
        for (int x= 0; x<NB_CELL_WIDTH; x++) {
            heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y);
            widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x);
            if(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus))                    
                pc.printf("  R  ");
            else{
                currProba=log_to_proba(map[x][y]);
                if ( currProba < 0.5)
                    pc.printf("  0  ");
                else{
                    if(currProba==0.5)
                        pc.printf("  .  ");
                    else
                        pc.printf("  +  ");
                } 
            }
        }
        pc.printf("\n\r");
    }
}


//MATHS heavy functions
/**********************************************************************/
//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));
}

//returns the probability [0,1] that the cell is occupied from the log valAue lt
float log_to_proba(float lt){
    return 1-1/(1+exp(lt));
}

//returns the log value that the cell is occupied from the probability value [0,1]
float proba_to_log(float p){
    return log(p/(1-p));
}

//returns the new log value
float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){
    return previousLogValue+proba_to_log(currentProbability)-originalLogvalue;
}

//makes the angle inAngle between 0 and 2pi
float rad_angle_check(float inAngle){
    //cout<<"before :"<<inAngle;
    if(inAngle > 0){
        while(inAngle > (2*pi))
            inAngle-=2*pi;
    }else{
        while(inAngle < 0)
            inAngle+=2*pi;
    }
    //cout<<" after :"<<inAngle<<endl;
    return inAngle;
}

//returns the angle between the vectors (x,y) and (xs,ys)
float compute_angle_between_vectors(float x, float y,float xs,float ys){
    //alpha angle between ->x and ->SA
    //vector S to A ->SA
    float vSAx=x-xs;
    float vSAy=y-ys;
    //norme SA
    float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2));
    //vector ->x (1,0)
    float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;;
    //vector ->y (0,1)
    float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx;
    if (sinAlpha < 0)
        return -acos(cosAlpha);
    else
        return acos(cosAlpha);
}

float robot_center_x_in_orthonormal_x(){
    return Y;
}

float robot_center_y_in_orthonormal_y(){
    return NB_CELL_WIDTH*sizeCellX-X;
}

float robot_sonar_front_x_in_orthonormal_x(){
    return Y+DISTANCE_SONAR_FRONT_Y;
}
float robot_sonar_front_y_in_orthonormal_y(){
    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X;
}

float robot_sonar_right_x_in_orthonormal_x(){
    return Y+DISTANCE_SONAR_RIGHT_Y;
}
float robot_sonar_right_y_in_orthonormal_y(){
    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X;
}

float robot_sonar_left_x_in_orthonormal_x(){
    return Y+DISTANCE_SONAR_LEFT_Y;
}
float robot_sonar_left_y_in_orthonormal_y(){
    return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X;
}

float estimated_width_indice_in_orthonormal_x(int i){
    return sizeCellX/2+i*sizeCellX;
}
float estimated_height_indice_in_orthonormal_y(int j){
    return sizeCellY/2+j*sizeCellY;
}