Mapping for TP2

Dependencies:   ISR_Mini-explorer mbed

Fork of GoToPointWithAngle by Georgios Tsamis

main.cpp.orig

Committer:
Ludwigfr
Date:
2017-04-24
Revision:
27:07bde633af72
Parent:
19:6a9062d54eb0

File content as of revision 27:07bde633af72:

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

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

int goToPointWithAngle(float target_x, float target_y, float target_angle);

int randomizeAndMap();
int updateSonarValues();
int computeObstacle();

bool map[25][25];
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;

bool tooClose = false;

int leftMm;
int frontMm;
int rightMm;


//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

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

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

    measure_always_on();
    wait_ms(50);
    
    //Fill map
    for (int i = 0; i<25; i++) {
        for (int j = 0; j<25; j++) {
            map[i][j] = false;        
        }
    }

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

    /*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;*/
    for (int i = 0; i<10; i++) {
        randomizeAndMap();
    }

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

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

//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));
}

//Updates sonar values
int updateSonarValues() {
    leftMm = get_distance_left_sensor();
    frontMm = get_distance_front_sensor();
    rightMm = get_distance_right_sensor();
    return 0;
}

int randomizeAndMap() {
    target_x = (rand()%2500)/10;//for decimal precision
    target_y = (rand()%2500)/10;
    target_angle = ((rand()%31416)-15708)/1000;
    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);
    goToPointWithAngle(target_x, target_y, target_angle);
    return 0;    
}

int computeObstacle() {
    //get the sensor values
    //compute the probabilistic shit of empty/non-empty in the direction of the sensor below 10 cm
    //update the map object with true where it's likely to be obstacled
    int xObstacle, yObstacle;
    if (leftMm < 10) {
    
    } else if (frontMm < 10) {
    
    } else if (rightMm < 10) {
    
    }
    
    map[xObstacle][yObstacle] = true;
    return 0;
}

int goToPointWithAngle(float target_x, float target_y, float target_angle) {
     do {
        pc.printf("\n\n\r entered while");
        
        //Timer stuff
        dt = t.read();
        t.reset();
        t.start();
        
        updateSonarValues();
        if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
            tooClose = true;    
        }
        
        //Updating X,Y and theta with the odometry values
        Odometria();
        
        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*b*angular)/r;
        angular_right=(linear+0.5*b*angular)/r;

        //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 leftMm=%f", leftMm);
        pc.printf("\n\r frontMm=%f", frontMm);
        pc.printf("\n\r rightMm=%f", rightMm);

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

        wait(0.2);
        //Timer stuff
        t.stop();
    } while(d2>1 & !tooClose);
    
    if (tooClose) {
        computeObstacle();
    }
    return 0;
}