Diff: main.cpp

Revision:

21:ebb37a249b5f

Parent:

20:62154d644531

Child:

23:901fc468b8a7

--- a/main.cpp	Fri Apr 07 15:47:51 2017 +0000
+++ b/main.cpp	Tue Apr 18 17:48:26 2017 +0000
@@ -1,19 +1,76 @@
 #include "mbed.h"
 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp!
 #include "math.h"
- 
-Timer t;
+
+ using namespace std;
 
-float dist(float robot_x, float robot_y, float target_x, float target_y);
+//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();
+//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();
 
-int goToPointWithAngle(float target_x, float target_y, float target_angle);
+//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);
 
-int randomizeAndMap();
-int updateSonarValues();
-int computeObstacle();
-int printFinalMap();
+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;//RADIUS_WHEELS
+const float RANGE_SONAR_MIN=10;//Rmin
+const float INCERTITUDE_SONAR=10;
+const float ANGLE_SONAR=pi/3;//Omega
+
+//those distance and angle are approximation in need of measurements
+const float ANGLE_FRONT_TO_LEFT=pi/3;
+const float DISTANCE_SONAR_LEFT_X=5;
+const float DISTANCE_SONAR_LEFT_Y=5;
+float leftMm;
+
+const float ANGLE_FRONT_TO_RIGHT=-pi/3;
+const float DISTANCE_SONAR_RIGHT_X=-5;
+const float DISTANCE_SONAR_RIGHT_Y=5;
+float rightMm;
 
-bool map[25][25];
+const float ANGLE_FRONT_TO_FRONT=0;
+const float DISTANCE_SONAR_FRONT_X=0;
+const float DISTANCE_SONAR_FRONT_Y=5;
+float frontMm;
+
+//TODO adjust the size of the map for computation time (25*25?)
+const int SIZE_MAP=25;
+
+//position and orientation of the robot when put on the map (ODOMETRY doesn't know those)
+const float DEFAULT_X=0;
+const float DEFAULT_Y=0;
+const float DEFAULT_THETA=pi/2;
+
+//used to create the map 250 represent the 250cm of the table where the robot is tested
+float sizeCell=250/(float)SIZE_MAP;
+float map[SIZE_MAP][SIZE_MAP];//contains the log values for each cell
+float initialLogValues[SIZE_MAP][SIZE_MAP];
+
+//Diameter of a wheel and distance between the 2
+const float RADIUS_WHEELS=3.25;
+const float DISTANCE_WHEELS=7.2;
+
 float alpha; //angle error
 float rho; //distance from target
 float beta;
@@ -22,121 +79,61 @@
 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;
+Timer t;
 
 int speed=999; // Max speed at beggining of movement
 
-//target exemple x y theta
-float target_x=46.8, target_y=78.6, target_angle=1.57;
-
-
-//Timeout time;
 int main(){
-    /*target_x=200*rand();
-    target_y=200*rand();
-    target_angle=3.1416*2*rand()-3.1416;*/
     
     i2c1.frequency(100000);
     initRobot(); //Initializing the robot
     pc.baud(9600); // baud for the pc communication
 
-    //measure_always_on();
-    //wait_ms(50);
+    measure_always_on();//TODO check if needed
     
-    //Fill map
-    for (int i = 0; i<25; i++) {
-        for (int j = 0; j<25; j++) {
-            map[i][j] = false;        
-        }
-    }
+    fill_initial_log_values();
 
-    //Resetting coordinates before moving
-    theta=0; 
-    X=125;
-    Y=125;
-
-    /*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<25; i++) {
-        randomizeAndMap();
+        randomize_and_map();
     }
     //Stop at the end
     leftMotor(1,0);
     rightMotor(1,0);
 
     //pc.printf("\n\r %f -- arrived!", rho);
-    printFinalMap();
+    print_final_map();
 }
 
-int printFinalMap() {
-    for (int i = 0; i<25; i++) {
-        for (int j = 0; j<25; j++) {
-            if (map[i][j]) {
-                pc.printf("X ");
-            } else {
-                pc.printf("O ");
-            }        
+//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<SIZE_MAP; i++) {
+        for (int j = 0; j<SIZE_MAP; j++) {
+            if(j==0 || j==SIZE_MAP-1 || i==0 || i==SIZE_MAP-1)
+                initialLogValues[i][j] = proba_to_log(1);
+            else
+                initialLogValues[i][j] = proba_to_log(0.5);
         }
-        pc.printf("\n\r");
-    }    
-    return 0;
-}
-//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() {
-    //start_read_left_sensor();
-    leftMm = get_distance_left_sensor();
-    //start_read_front_sensor();
-    frontMm = get_distance_front_sensor();
-    //start_read_right_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 = ((float)(rand()%31416)-15708)/10000.0;
+//generate a position randomly and makes the robot go there while updating the map
+void randomize_and_map() {
+    float target_x = (rand()%2500)/10;//for decimal precision
+    float target_y = (rand()%2500)/10;
+    float target_angle = ((float)(rand()%31416)-15708)/10000.0;
+    /*
     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;    
+    */
+    go_to_point_with_angle(target_x, target_y, target_angle);
 }
 
-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) {
+//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) {
+    bool tooClose=false;
      do {
         pc.printf("\n\n\r entered while");
         
@@ -148,7 +145,8 @@
         //Updating X,Y and theta with the odometry values
         Odometria();
         
-        updateSonarValues();
+        update_sonar_values();
+
         if (leftMm < 100 || frontMm < 100 || rightMm < 100) {
             tooClose = true;
             break;    
@@ -177,8 +175,8 @@
             linear=-kRho*rho;
             angular=-ka*alpha-kb*beta;
         }
-        angular_left=(linear-0.5*b*angular)/r;
-        angular_right=(linear+0.5*b*angular)/r;
+        angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
+        angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS;
 
         //Slowing down at the end for more precision
         if (d2<25) {
@@ -207,10 +205,151 @@
         wait(0.2);
         //Timer stuff
         t.stop();
-    } while(d2>1);
-    
-    if (tooClose) {
-        computeObstacle();
+    } while(d2>1 && tooClose==false);
+}
+
+//Updates sonar values
+void update_sonar_values(){
+
+    float leftMm = get_distance_left_sensor();
+    float frontMm = get_distance_front_sensor();
+    float rightMm = get_distance_right_sensor();
+
+    float currProba;
+    for(int i=0;i<SIZE_MAP;i++){
+        for(int j=0;j<SIZE_MAP;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)
+
+            //compute for front sonar
+            currProba=compute_probability_t(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_FRONT_X,Y+DEFAULT_Y+DISTANCE_SONAR_FRONT_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(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_RIGHT_X,Y+DEFAULT_Y+DISTANCE_SONAR_RIGHT_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(sizeCell/2+i*sizeCell,sizeCell/2+j*sizeCell,SIZE_MAP*sizeCell-X+DEFAULT_X+DISTANCE_SONAR_LEFT_X,Y+DEFAULT_Y+DISTANCE_SONAR_LEFT_Y,ANGLE_FRONT_TO_LEFT,leftMm/10);
+            map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j];
+        }
+    }
+}
+
+//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-DEFAULT_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 in range of the sonar
+        return 0.5;
     }
-    return 0;
-}
\ No newline at end of file
+}
+
+void print_final_map() {
+    float currProba;
+    for (int x = 0; x<SIZE_MAP; x++) {
+        for (int y = 0; y<SIZE_MAP; y++) {
+                currProba=log_to_proba(map[SIZE_MAP-1-x][y]);
+            if ( currProba < 0.5) {//by default [0][0] would be top left, we want it bottom left
+                pc.printf("0");
+            } else {
+                if(currProba==0.5)
+                    pc.printf(".");
+                else
+                    pc.printf("+");
+            }
+        }
+    }
+}
+
+
+
+
+//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 value 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);
+}
+