![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 22:ebb37a249b5f
- Parent:
- 21: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); +} +