![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Mapping for TP2
Dependencies: ISR_Mini-explorer mbed
Fork of GoToPointWithAngle by
Diff: main.cpp
- Revision:
- 24:8f4b820d8de8
- Parent:
- 23:901fc468b8a7
- Child:
- 25:572c9e9a8809
--- a/main.cpp Wed Apr 19 10:45:09 2017 +0000 +++ b/main.cpp Wed Apr 19 22:20:56 2017 +0000 @@ -11,16 +11,11 @@ //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(); +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(); -//compute the angles and distance used for the velocities -void compute_angles_and_distance(float target_x, float target_y, float target_angle); -//compute the linear and 2 angular velocities -void compute_linear_angular_velocities(); - //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -38,55 +33,54 @@ 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 +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=pi/3; -const float DISTANCE_SONAR_LEFT_X=5; -const float DISTANCE_SONAR_LEFT_Y=5; -float leftMm; +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=-pi/3; -const float DISTANCE_SONAR_RIGHT_X=-5; -const float DISTANCE_SONAR_RIGHT_Y=5; -float rightMm; +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; -float frontMm; //TODO adjust the size of the map for computation time (25*25?) -const int SIZE_MAP=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=0; -const float DEFAULT_Y=0; +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 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]; +//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; -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; +const int MAX_SPEED=500;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP -int speed=999; // Max speed at beggining of movement +bool tooClose=false; +bool turnFromObstacle=false; int main(){ @@ -95,26 +89,28 @@ pc.baud(9600); // baud for the pc communication measure_always_on();//TODO check if needed + wait(0.5); fill_initial_log_values(); - for (int i = 0; i<25; i++) { + theta=DEFAULT_THETA; + X=DEFAULT_X; + Y=DEFAULT_Y; + + for (int i = 0; i<20; i++) { randomize_and_map(); + wait(2); + print_final_map(); } - //Stop at the end - leftMotor(1,0); - rightMotor(1,0); - //pc.printf("\n\r %f -- arrived!", rho); - 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<SIZE_MAP; i++) { - for (int j = 0; j<SIZE_MAP; j++) { - if(j==0 || j==SIZE_MAP-1 || i==0 || i==SIZE_MAP-1) + 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); @@ -124,23 +120,56 @@ //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; + //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; - /* - 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); - */ + + //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) { - bool tooClose=false; //binary variable to know if an obstacle is <10cm away - do { - pc.printf("\n\n\r entered while"); + 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(); @@ -149,25 +178,76 @@ //Updating X,Y and theta with the odometry values Odometria(); - - //Updare the values from the sonar - update_sonar_values(); + + 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); - //Check if one of the sonars finds an obstacle that is too close - if (leftMm < 100 || frontMm < 100 || rightMm < 100) { + 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; } - compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target - compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular) + 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); - //Printing values for debugging purposes - 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); + //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); @@ -176,29 +256,34 @@ wait(0.2); //Timer stuff t.stop(); - } while(d2>1 && tooClose==false); + } 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 = get_distance_left_sensor(); - float frontMm = get_distance_front_sensor(); - float rightMm = get_distance_right_sensor(); +void update_sonar_values(float leftMm, float frontMm, float rightMm){ 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) - + 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 - 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); + currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_FRONT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X,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); + currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_RIGHT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X,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); + currProba=compute_probability_t(sizeCellX/2+i*sizeCellX,sizeCellY/2+j*sizeCellY,Y+DISTANCE_SONAR_LEFT_Y,NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X,ANGLE_FRONT_TO_LEFT,leftMm/10); map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; } } @@ -208,7 +293,7 @@ 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; + 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)); @@ -250,24 +335,48 @@ } } -void print_final_map() { +void print_final_map_1() { 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"); + 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("."); + pc.printf(" . "); else - pc.printf("+"); + pc.printf(" + "); } } + pc.printf("\n\r"); } } - +void print_final_map_2() { + 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(x => && x <= && y=> && y<= ){ + pc.printf(" R "); + }else{ + if ( currProba < 0.5) { + pc.printf(" 0 "); + } else { + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" + "); + } + } + + } + pc.printf("\n\r"); + } +} //MATHS heavy functions @@ -277,7 +386,7 @@ 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 +//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)); } @@ -322,48 +431,4 @@ return -acos(cosAlpha); else return acos(cosAlpha); -} - -void compute_angles_and_distance(float target_x, float target_y, float target_angle){ - 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); -} - -void compute_linear_angular_velocities(){ - //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; - - //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; - } -} +} \ No newline at end of file