Georgios Tsamis
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of
VirtualForces
by 
Georgios Tsamis
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 #include "mbed.h" 00002 #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! 00003 #include "math.h" 00004 00005 using namespace std; 00006 00007 //makes the angle inAngle between pi and minus pi 00008 float rad_angle_check_pi_and_minus_pi(float inAngle); 00009 void initialise_parameters(); 00010 //fill initialLogValues with the values we already know (here the bordurs) 00011 void fill_initial_log_values(); 00012 //generate a position randomly and makes the robot go there while updating the map 00013 void randomize_and_map(); 00014 //make the robot do a pi/2 flip 00015 void do_half_flip(); 00016 //go the the given position while updating the map 00017 void go_to_point_with_angle(float target_x, float target_y, float target_angle); 00018 //Updates sonar values 00019 void update_sonar_values(float leftMm, float frontMm, float rightMm); 00020 //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] 00021 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected); 00022 //print the map 00023 void print_final_map(); 00024 //print the map with the robot marked on it 00025 void print_final_map_with_robot_position(); 00026 //print the map with the robot and the target marked on it 00027 void print_final_map_with_robot_position_and_target(); 00028 //go to a given line by updating angularLeft and angularRight 00029 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c); 00030 //calculate virtual force field and move 00031 void vff(bool* reached); 00032 void test_got_to_line(bool* reached); 00033 00034 //MATHS heavy functions 00035 float dist(float robot_x, float robot_y, float target_x, float target_y); 00036 //returns the probability [0,1] that the cell is occupied from the log value lt 00037 float log_to_proba(float lt); 00038 //returns the log value that the cell is occupied from the probability value [0,1] 00039 float proba_to_log(float p); 00040 //returns the new log value 00041 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ); 00042 //makes the angle inAngle between 0 and 2pi 00043 float rad_angle_check(float inAngle); 00044 //returns the angle between the vectors (x,y) and (xs,ys) 00045 float compute_angle_between_vectors(float x, float y,float xs,float ys); 00046 float x_robot_in_orthonormal_x(float x, float y); 00047 float y_robot_in_orthonormal_y(float x, float y); 00048 float robot_center_x_in_orthonormal_x(); 00049 float robot_center_y_in_orthonormal_y(); 00050 float robot_sonar_front_x_in_orthonormal_x(); 00051 float robot_sonar_front_y_in_orthonormal_y(); 00052 float robot_sonar_right_x_in_orthonormal_x(); 00053 float robot_sonar_right_y_in_orthonormal_y(); 00054 float robot_sonar_left_x_in_orthonormal_x(); 00055 float robot_sonar_left_y_in_orthonormal_y(); 00056 float estimated_width_indice_in_orthonormal_x(int i); 00057 float estimated_height_indice_in_orthonormal_y(int j); 00058 //update angleError,distanceFromTarget,d2, beta 00059 void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta); 00060 //update angularLeft and angularRight 00061 void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta, float* angularLeft, float* angularRight); 00062 //update foceX and forceY if necessary 00063 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); 00064 //compute the X and Y force 00065 void compute_forceX_and_forceY(float* forceX, float* forceY); 00066 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c 00067 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c); 00068 //return 1 if positiv, -1 if negativ 00069 float sign1(float value); 00070 //return 1 if positiv, 0 if negativ 00071 int sign2(float value); 00072 //set target in ortho space, in reference to the robot (so if the robot is in the middle, you want to him to go 10cm down and 15 right, set_target_to(15,-10) 00073 void set_target_to(float x, float y); 00074 void try_to_reach_target(); 00075 00076 const float pi=3.14159; 00077 00078 //spec of the sonar 00079 //TODO MEASURE THE DISTANCE on X and Y of the robot space, between each sonar and the center of the robot and add it to calculus in updateSonarValues 00080 const float RANGE_SONAR=50;//cm 00081 const float RANGE_SONAR_MIN=10;//Rmin cm 00082 const float INCERTITUDE_SONAR=10;//cm 00083 const float ANGLE_SONAR=pi/3;//Omega rad 00084 00085 //those distance and angle are approximation in need of measurements, in the orthonormal space 00086 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees 00087 const float DISTANCE_SONAR_LEFT_X=-4; 00088 const float DISTANCE_SONAR_LEFT_Y=4; 00089 00090 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees 00091 const float DISTANCE_SONAR_RIGHT_X=4; 00092 const float DISTANCE_SONAR_RIGHT_Y=4; 00093 00094 const float ANGLE_FRONT_TO_FRONT=0; 00095 const float DISTANCE_SONAR_FRONT_X=0; 00096 const float DISTANCE_SONAR_FRONT_Y=5; 00097 00098 //TODO adjust the size of the map for computation time (25*25?) 00099 const float WIDTH_ARENA=120;//cm 00100 const float HEIGHT_ARENA=90;//cm 00101 00102 //const int SIZE_MAP=25; 00103 const int NB_CELL_WIDTH=24; 00104 const int NB_CELL_HEIGHT=18; 00105 00106 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space 00107 //this configuration suppose that the robot is in the middle of the arena facing up (to be sure you can use print_final_map_with_robot_position 00108 //const float DEFAULT_X=HEIGHT_ARENA/2; 00109 //const float DEFAULT_Y=WIDTH_ARENA/2; 00110 const float DEFAULT_X=20;//lower right 00111 const float DEFAULT_Y=20;//lower right 00112 const float DEFAULT_THETA=0; 00113 00114 //used to create the map 250 represent the 250cm of the square where the robot is tested 00115 //float sizeCell=250/(float)SIZE_MAP; 00116 float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH; 00117 float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT; 00118 00119 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell 00120 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT]; 00121 00122 //Diameter of a wheel and distance between the 2 00123 const float RADIUS_WHEELS=3.25; 00124 const float DISTANCE_WHEELS=7.2; 00125 00126 const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP 00127 00128 //TODO all those global variables are making me sad 00129 const float KRHO=12, KA=30, KB=-13, KV=150, KH=150; //Kappa values 00130 00131 //CONSTANT FORCE FIELD 00132 const float FORCE_CONSTANT_REPULSION=80;//TODO tweak it 00133 const float FORCE_CONSTANT_ATTRACTION=25;//TODO tweak it 00134 const float RANGE_FORCE=50;//TODO tweak it 00135 00136 //those target are in comparison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move) 00137 float targetX;//this is in the robot space top left 00138 float targetY;//this is in the robot space top left 00139 float targetXOrtho; 00140 float targetYOrtho; 00141 00142 bool direction; 00143 00144 int main(){ 00145 initialise_parameters(); 00146 //try to reach the target 00147 set_target_to(0,50);//up right 00148 print_final_map_with_robot_position_and_target(); 00149 try_to_reach_target(); 00150 set_target_to(0,-50);//lower right 00151 print_final_map_with_robot_position_and_target(); 00152 try_to_reach_target(); 00153 set_target_to(-50,50);//up left 00154 print_final_map_with_robot_position_and_target(); 00155 try_to_reach_target(); 00156 //print the map forever 00157 while(1){ 00158 print_final_map_with_robot_position_and_target(); 00159 } 00160 } 00161 00162 void try_to_reach_target(){ 00163 bool reached=false; 00164 int print=0; 00165 while (!reached) { 00166 vff(&reached); 00167 //test_got_to_line(&reached); 00168 if(print==10){ 00169 leftMotor(1,0); 00170 rightMotor(1,0); 00171 pc.printf("\r\n theta=%f", theta); 00172 pc.printf("\r\n IN ORTHO:"); 00173 pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x()); 00174 pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y()); 00175 pc.printf("\r\n X Target=%f", targetXOrtho); 00176 pc.printf("\r\n Y Target=%f", targetYOrtho); 00177 print_final_map_with_robot_position_and_target(); 00178 print=0; 00179 }else 00180 print+=1; 00181 } 00182 //Stop at the end 00183 leftMotor(1,0); 00184 rightMotor(1,0); 00185 pc.printf("\r\n target reached"); 00186 } 00187 00188 //target in ortho space 00189 void set_target_to(float x, float y){ 00190 targetX=y; 00191 targetY=-x; 00192 targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY); 00193 targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY); 00194 00195 pc.printf("\r\nangletarget= %f", atan2(x,y)); 00196 float angleError = atan2(x,y)-theta; 00197 if(angleError>pi) angleError-=2*pi; 00198 if(angleError<-pi) angleError+=2*pi; 00199 00200 if(angleError<(pi/2) && angleError>(-pi/2)) direction=true; 00201 else direction=false; 00202 pc.printf("\r\nangleError= %f", angleError); 00203 } 00204 00205 void initialise_parameters(){ 00206 i2c1.frequency(100000); 00207 initRobot(); //Initializing the robot 00208 pc.baud(9600); // baud for the pc communication 00209 00210 measure_always_on();//TODO check if needed 00211 wait(0.5); 00212 //fill the map with the initial log values 00213 fill_initial_log_values(); 00214 00215 theta=DEFAULT_THETA; 00216 X=DEFAULT_X; 00217 Y=DEFAULT_Y; 00218 } 00219 00220 //fill initialLogValues with the values we already know (here the bordurs) 00221 void fill_initial_log_values(){ 00222 //Fill map, we know the border are occupied 00223 for (int i = 0; i<NB_CELL_WIDTH; i++) { 00224 for (int j = 0; j<NB_CELL_HEIGHT; j++) { 00225 if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1) 00226 initialLogValues[i][j] = proba_to_log(1); 00227 else 00228 initialLogValues[i][j] = proba_to_log(0.5); 00229 } 00230 } 00231 } 00232 00233 //generate a position randomly and makes the robot go there while updating the map 00234 void randomize_and_map() { 00235 //TODO check that it's aurelien's work 00236 float target_x = (rand()%(int)(HEIGHT_ARENA*10))/10;//for decimal precision 00237 float target_y = (rand()%(int)(WIDTH_ARENA*10))/10; 00238 float target_angle = 2*((float)(rand()%31416)-15708)/10000.0; 00239 00240 go_to_point_with_angle(targetX, targetY, target_angle); 00241 } 00242 00243 00244 void do_half_flip(){ 00245 Odometria(); 00246 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi 00247 if(theta_plus_h_pi > pi) 00248 theta_plus_h_pi=-(2*pi-theta_plus_h_pi); 00249 leftMotor(0,100); 00250 rightMotor(1,100); 00251 while(abs(theta_plus_h_pi-theta)>0.05){ 00252 Odometria(); 00253 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta)); 00254 } 00255 leftMotor(1,0); 00256 rightMotor(1,0); 00257 } 00258 00259 //go the the given position while updating the map 00260 //TODO clean this procedure it's ugly as hell and too long 00261 void go_to_point_with_angle(float target_x, float target_y, float target_angle) { 00262 set_target_to(target_x,target_y); 00263 Odometria(); 00264 float angleError = atan2((target_y-Y),(target_x-X))-theta; 00265 if(!cos(angleError)) 00266 angleError = atan(sin(angleError)/cos(angleError)); 00267 else 00268 angleError=pi/2; 00269 if(isnan(angleError)) pc.printf("\r\n nan line 264"); 00270 float distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho); 00271 float beta = -angleError-theta+target_angle; 00272 //beta = atan(sin(beta)/cos(beta)); 00273 bool keep_going=true; 00274 float leftMm; 00275 float frontMm; 00276 float rightMm; 00277 float angularLeft=0; 00278 float angularRight=0; 00279 Timer t; 00280 float dt=0.5;//TODO better name please 00281 float d2;//TODO better name please 00282 do { 00283 //Timer stuff 00284 dt = t.read(); 00285 t.reset(); 00286 t.start(); 00287 00288 //Updating X,Y and theta with the odometry values 00289 Odometria(); 00290 leftMm = get_distance_left_sensor(); 00291 frontMm = get_distance_front_sensor(); 00292 rightMm = get_distance_right_sensor(); 00293 00294 //pc.printf("\n\r leftMm=%f", leftMm); 00295 //pc.printf("\n\r frontMm=%f", frontMm); 00296 //pc.printf("\n\r rightMm=%f", rightMm); 00297 00298 //if in dangerzone 00299 if(frontMm < 120 || leftMm <120 || rightMm <120){ 00300 leftMotor(1,0); 00301 rightMotor(1,0); 00302 update_sonar_values(leftMm, frontMm, rightMm); 00303 //TODO Giorgos maybe you can also test the do_half_flip() function 00304 Odometria(); 00305 //do a flip TODO 00306 keep_going=false; 00307 do_half_flip(); 00308 }else{ 00309 //if not in danger zone continue as usual 00310 update_sonar_values(leftMm, frontMm, rightMm); 00311 compute_angles_and_distance(target_x, target_y, target_angle,dt,&angleError,&distanceFromTarget,&d2,&beta);//Compute the angles and the distance from target 00312 compute_linear_angular_velocities(angleError,distanceFromTarget,beta,&angularLeft,&angularRight); //Using the angles and distance, compute the velocities needed (linear & angular) 00313 00314 //pc.printf("\n\r X=%f", X); 00315 //pc.printf("\n\r Y=%f", Y); 00316 00317 //pc.printf("\n\r a_r=%f", angularRight); 00318 //pc.printf("\n\r a_l=%f", angularLeft); 00319 00320 //Updating motor velocities 00321 leftMotor(sign2(angularLeft),abs(angularLeft)); 00322 rightMotor(sign2(angularRight),abs(angularRight)); 00323 00324 wait(0.2); 00325 //Timer stuff 00326 t.stop(); 00327 } 00328 } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going); 00329 00330 //Stop at the end 00331 leftMotor(1,0); 00332 rightMotor(1,0); 00333 } 00334 00335 //Updates sonar values 00336 void update_sonar_values(float leftMm, float frontMm, float rightMm){ 00337 float currProba; 00338 float i_in_orthonormal; 00339 float j_in_orthonormal; 00340 for(int i=0;i<NB_CELL_WIDTH;i++){ 00341 for(int j=0;j<NB_CELL_HEIGHT;j++){ 00342 //check if the point A(x,y) in the world space is within the range of the sonar (which has the coordinates xs, ys in the world space) 00343 //check that again 00344 //compute for front sonar 00345 i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i); 00346 j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j); 00347 00348 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); 00349 if(isnan(currProba)) pc.printf("\r\n currProba is nan"); 00350 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 00351 //compute for right sonar 00352 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); 00353 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; 00354 //compute for left sonar 00355 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); 00356 if(isnan(currProba)) pc.printf("\r\n nan line 354"); 00357 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; 00358 if(isnan(proba_to_log(currProba))) pc.printf("\r\nnan in line 355"); 00359 } 00360 } 00361 } 00362 00363 //makes the angle inAngle between pi and minus pi 00364 float rad_angle_check_pi_and_minus_pi(float inAngle){ 00365 //cout<<"before :"<<inAngle; 00366 if(inAngle > 0){ 00367 while(inAngle > (pi)) 00368 inAngle-=pi; 00369 }else{ 00370 while(inAngle < 0) 00371 inAngle+=pi; 00372 } 00373 //cout<<" after :"<<inAngle<<endl; 00374 return inAngle; 00375 } 00376 00377 //ODOMETRIA MUST HAVE BEEN CALLED 00378 //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] 00379 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){ 00380 00381 float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam 00382 float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition; 00383 anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure 00384 alphaBeforeAdjustment=rad_angle_check_pi_and_minus_pi(alphaBeforeAdjustment); 00385 //if(abs(alphaBeforeAdjustment)>ANGLE_SONAR/2) pc.printf("\r\n it is!"); 00386 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); 00387 00388 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS 00389 //check if absolute difference between the angles is no more than Omega/2 00390 if( distancePointToSonar < (RANGE_SONAR)&& (anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2))){ 00391 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){ 00392 //point before obstacle, probably empty 00393 /*****************************************************************************/ 00394 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); 00395 float Er; 00396 if(distancePointToSonar < RANGE_SONAR_MIN){ 00397 //point before minimum sonar range 00398 Er=0.f; 00399 }else{ 00400 //point after minimum sonar range 00401 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2); 00402 } 00403 /*****************************************************************************/ 00404 if((1.f-Er*Ea)/2.f>1) pc.printf("\r\n E>1"); 00405 if((1.f-Er*Ea)/2.f<0) pc.printf("\r\n E<0"); 00406 return (1.f-Er*Ea)/2.f; 00407 }else{ 00408 //probably occupied 00409 /*****************************************************************************/ 00410 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); 00411 float Or; 00412 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){ 00413 //point between distanceObstacleDetected +- INCERTITUDE_SONAR 00414 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2); 00415 }else{ 00416 //point after in range of the sonar but after the zone detected 00417 Or=0; 00418 } 00419 /*****************************************************************************/ 00420 if((1+Or*Oa)/2>1) pc.printf("\r\n O>1"); 00421 if((1+Or*Oa)/2<0) pc.printf("\r\n O<0"); 00422 return (1+Or*Oa)/2; 00423 } 00424 } 00425 //not checked by the sonar 00426 return 0.5; 00427 } 00428 00429 void print_final_map() { 00430 float currProba; 00431 pc.printf("\n\r"); 00432 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00433 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00434 currProba=log_to_proba(map[x][y]); 00435 if ( currProba < 0.5) { 00436 pc.printf(" "); 00437 } else { 00438 if(currProba==0.5) 00439 pc.printf(" . "); 00440 else 00441 pc.printf(" X "); 00442 } 00443 } 00444 pc.printf("\n\r"); 00445 } 00446 } 00447 00448 void print_final_map_with_robot_position() { 00449 float currProba; 00450 Odometria(); 00451 float Xrobot=robot_center_x_in_orthonormal_x(); 00452 float Yrobot=robot_center_y_in_orthonormal_y(); 00453 00454 float heightIndiceInOrthonormal; 00455 float widthIndiceInOrthonormal; 00456 00457 float widthMalus=-(3*sizeCellWidth/2); 00458 float widthBonus=sizeCellWidth/2; 00459 00460 float heightMalus=-(3*sizeCellHeight/2); 00461 float heightBonus=sizeCellHeight/2; 00462 00463 pc.printf("\n\r"); 00464 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00465 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00466 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); 00467 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); 00468 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) 00469 pc.printf(" R "); 00470 else{ 00471 currProba=log_to_proba(map[x][y]); 00472 if ( currProba < 0.5) 00473 pc.printf(" "); 00474 else{ 00475 if(currProba==0.5) 00476 pc.printf(" . "); 00477 else 00478 pc.printf(" X "); 00479 } 00480 } 00481 } 00482 pc.printf("\n\r"); 00483 } 00484 } 00485 00486 void print_final_map_with_robot_position_and_target() { 00487 float currProba; 00488 Odometria(); 00489 float Xrobot=robot_center_x_in_orthonormal_x(); 00490 float Yrobot=robot_center_y_in_orthonormal_y(); 00491 00492 float heightIndiceInOrthonormal; 00493 float widthIndiceInOrthonormal; 00494 00495 float widthMalus=-(3*sizeCellWidth/2); 00496 float widthBonus=sizeCellWidth/2; 00497 00498 float heightMalus=-(3*sizeCellHeight/2); 00499 float heightBonus=sizeCellHeight/2; 00500 00501 pc.printf("\n\r"); 00502 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00503 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00504 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); 00505 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); 00506 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) 00507 pc.printf(" R "); 00508 else{ 00509 if(targetYOrtho >= (heightIndiceInOrthonormal+heightMalus) && targetYOrtho <= (heightIndiceInOrthonormal+heightBonus) && targetXOrtho >= (widthIndiceInOrthonormal+widthMalus) && targetXOrtho <= (widthIndiceInOrthonormal+widthBonus)) 00510 pc.printf(" T "); 00511 else{ 00512 currProba=log_to_proba(map[x][y]); 00513 if ( currProba < 0.5) 00514 pc.printf(" "); 00515 else{ 00516 if(currProba==0.5) 00517 pc.printf(" . "); 00518 else 00519 pc.printf(" X "); 00520 } 00521 } 00522 } 00523 } 00524 pc.printf("\n\r"); 00525 } 00526 } 00527 00528 //MATHS heavy functions 00529 /**********************************************************************/ 00530 //Distance computation function 00531 float dist(float robot_x, float robot_y, float target_x, float target_y){ 00532 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); 00533 } 00534 00535 //returns the probability [0,1] that the cell is occupied from the log value lt 00536 float log_to_proba(float lt){ 00537 float temp=1-1/(1+exp(lt)); 00538 if(isnan(temp)){ 00539 //pc.printf("\r\n nan in line 514"); 00540 //pc.printf("\r\nlt= %f, 1+exp(lt)= %f", lt, 1+exp(lt)); 00541 } 00542 return temp; 00543 } 00544 00545 //returns the log value that the cell is occupied from the probability value [0,1] 00546 float proba_to_log(float p){ 00547 float temp; 00548 if(p==1) temp=log(0.99/(1-0.99)); 00549 else temp=log(p/(1-p)); 00550 if(isnan(temp)) pc.printf("\r\n temp=%f, p=%f", temp,p); 00551 return temp; 00552 } 00553 00554 //returns the new log value 00555 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){ 00556 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue; 00557 } 00558 00559 //makes the angle inAngle between 0 and 2pi 00560 float rad_angle_check(float inAngle){ 00561 //cout<<"before :"<<inAngle; 00562 if(inAngle > 0){ 00563 while(inAngle > (2*pi)) 00564 inAngle-=2*pi; 00565 }else{ 00566 while(inAngle < 0) 00567 inAngle+=2*pi; 00568 } 00569 //cout<<" after :"<<inAngle<<endl; 00570 return inAngle; 00571 } 00572 00573 //returns the angle between the vectors (x,y) and (xs,ys) 00574 float compute_angle_between_vectors(float x, float y,float xs,float ys){ 00575 //alpha angle between ->x and ->SA 00576 //vector S to A ->SA 00577 float vSAx=x-xs; 00578 float vSAy=y-ys; 00579 //norme SA 00580 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2)); 00581 //vector ->x (1,0) 00582 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;; 00583 //vector ->y (0,1) 00584 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx; 00585 if (sinAlpha < 0) 00586 return -acos(cosAlpha); 00587 else 00588 return acos(cosAlpha); 00589 } 00590 /* 00591 00592 00593 Robot space: orthonormal space: 00594 ^ ^ 00595 |x |y 00596 <- R O -> 00597 y x 00598 */ 00599 //Odometria must bu up to date 00600 float x_robot_in_orthonormal_x(float x, float y){ 00601 return robot_center_x_in_orthonormal_x()-y; 00602 } 00603 00604 //Odometria must bu up to date 00605 float y_robot_in_orthonormal_y(float x, float y){ 00606 return robot_center_y_in_orthonormal_y()+x; 00607 } 00608 00609 float robot_center_x_in_orthonormal_x(){ 00610 return NB_CELL_WIDTH*sizeCellWidth-Y; 00611 } 00612 00613 float robot_center_y_in_orthonormal_y(){ 00614 return X; 00615 } 00616 00617 float robot_sonar_front_x_in_orthonormal_x(){ 00618 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X; 00619 } 00620 float robot_sonar_front_y_in_orthonormal_y(){ 00621 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y; 00622 } 00623 00624 float robot_sonar_right_x_in_orthonormal_x(){ 00625 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X; 00626 } 00627 float robot_sonar_right_y_in_orthonormal_y(){ 00628 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y; 00629 } 00630 00631 float robot_sonar_left_x_in_orthonormal_x(){ 00632 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X; 00633 } 00634 float robot_sonar_left_y_in_orthonormal_y(){ 00635 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y; 00636 } 00637 00638 float estimated_width_indice_in_orthonormal_x(int i){ 00639 return sizeCellWidth/2+i*sizeCellWidth; 00640 } 00641 float estimated_height_indice_in_orthonormal_y(int j){ 00642 return sizeCellHeight/2+j*sizeCellHeight; 00643 } 00644 00645 //update angleError,distanceFromTarget,d2, beta 00646 void compute_angles_and_distance(float target_x, float target_y, float target_angle,float dt,float* angleError,float* distanceFromTarget,float* d2,float* beta){ 00647 *angleError = atan2((target_y-Y),(target_x-X))-theta; 00648 if(!cos(*angleError)) 00649 *angleError = atan(sin(*angleError)/cos(*angleError)); 00650 else 00651 *angleError=pi/2; 00652 if(isnan(*angleError)) pc.printf("\r\n nan line 613"); 00653 *distanceFromTarget = dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho); 00654 *d2 = *distanceFromTarget; 00655 *beta = -*angleError-theta+target_angle; 00656 00657 //Computing angle error and distance towards the target value 00658 *distanceFromTarget += dt*(-KRHO*cos(*angleError)**distanceFromTarget); 00659 float temp = *angleError; 00660 *angleError += dt*(KRHO*sin(*angleError)-KA**angleError-KB**beta); 00661 *beta += dt*(-KRHO*sin(temp)); 00662 //pc.printf("\n\r d2=%f", d2); 00663 //pc.printf("\n\r dt=%f", dt); 00664 } 00665 00666 //update angularLeft and angularRight 00667 void compute_linear_angular_velocities(float angleError,float distanceFromTarget,float beta,float* angularLeft, float* angularRight){ 00668 //Computing linear and angular velocities 00669 float linear; 00670 float angular; 00671 if(angleError>=-1.5708 && angleError<=1.5708){ 00672 linear=KRHO*distanceFromTarget; 00673 angular=KA*angleError+KB*beta; 00674 } 00675 else{ 00676 linear=-KRHO*distanceFromTarget; 00677 angular=-KA*angleError-KB*beta; 00678 } 00679 //TODO check those signs 00680 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00681 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00682 00683 float aL=*angularLeft; 00684 float aR=*angularRight; 00685 //Normalize speed for motors 00686 if(abs(*angularLeft)>abs(*angularRight)) { 00687 *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR); 00688 *angularLeft=MAX_SPEED*sign1(aL); 00689 } 00690 else { 00691 *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL); 00692 *angularRight=MAX_SPEED*sign1(aR); 00693 } 00694 } 00695 00696 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ 00697 //get the coordonate of the map and the robot in the ortonormal space 00698 float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice); 00699 float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice); 00700 //compute the distance beetween the cell and the robot 00701 float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2)); 00702 //check if the cell is in range 00703 if(distanceCellToRobot <= range) { 00704 float probaCell=log_to_proba(map[widthIndice][heightIndice]); 00705 //if(isnan(probaCell)) pc.printf("\r\nnan in probaCell"); 00706 float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3); 00707 //if(isnan(xForceComputed)) pc.printf("\r\nnan in line 673"); 00708 float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3); 00709 //if(isnan(yForceComputed)) pc.printf("\r\nnan in line 675"); 00710 *forceX+=xForceComputed; 00711 *forceY+=yForceComputed; 00712 } 00713 } 00714 00715 //compute the force on X and Y 00716 void compute_forceX_and_forceY(float* forceX, float* forceY){ 00717 //we put the position of the robot in an orthonormal space 00718 float xRobotOrtho=robot_center_x_in_orthonormal_x(); 00719 float yRobotOrtho=robot_center_y_in_orthonormal_y(); 00720 00721 float forceRepulsionComputedX=0; 00722 float forceRepulsionComputedY=0; 00723 //for each cell of the map we compute a force of repulsion 00724 for(int i=0;i<NB_CELL_WIDTH;i++){ 00725 for(int j=0;j<NB_CELL_HEIGHT;j++){ 00726 update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); 00727 } 00728 } 00729 //update with attraction force 00730 *forceX=-forceRepulsionComputedX; 00731 *forceY=-forceRepulsionComputedY; 00732 float distanceTargetRobot=sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); 00733 if(distanceTargetRobot != 0){ 00734 *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/distanceTargetRobot; 00735 *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/distanceTargetRobot; 00736 } 00737 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); 00738 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 00739 *forceX=*forceX/amplitude; 00740 *forceY=*forceY/amplitude; 00741 } 00742 } 00743 00744 void test_got_to_line(bool* reached){ 00745 float line_a=1; 00746 float line_b=2; 00747 float line_c=-140; 00748 //we update the odometrie 00749 Odometria(); 00750 float angularRight=0; 00751 float angularLeft=0; 00752 00753 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); 00754 pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); 00755 00756 leftMotor(sign2(angularLeft),abs(angularLeft)); 00757 rightMotor(sign2(angularRight),abs(angularRight)); 00758 00759 pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)); 00760 00761 //wait(0.1); 00762 Odometria(); 00763 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10) 00764 *reached=true; 00765 } 00766 void vff(bool* reached){ 00767 float line_a=0; 00768 float line_b=0; 00769 float line_c=0; 00770 //Updating X,Y and theta with the odometry values 00771 float forceX=0; 00772 float forceY=0; 00773 //we update the odometrie 00774 Odometria(); 00775 //we check the sensors 00776 float leftMm = get_distance_left_sensor(); 00777 float frontMm = get_distance_front_sensor(); 00778 float rightMm = get_distance_right_sensor(); 00779 float angularRight=0; 00780 float angularLeft=0; 00781 //update the probabilities values 00782 update_sonar_values(leftMm, frontMm, rightMm); 00783 //we compute the force on X and Y 00784 compute_forceX_and_forceY(&forceX, &forceY); 00785 //we compute a new ine 00786 calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c); 00787 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); 00788 00789 //Updating motor velocities 00790 00791 leftMotor(sign2(angularLeft),abs(angularLeft)); 00792 rightMotor(sign2(angularRight),abs(angularRight)); 00793 00794 //wait(0.1); 00795 Odometria(); 00796 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrtho,targetYOrtho)<10) 00797 *reached=true; 00798 } 00799 00800 //return 1 if positiv, -1 if negativ 00801 float sign1(float value){ 00802 if(value>=0) 00803 return 1; 00804 else 00805 return -1; 00806 } 00807 00808 //return 1 if positiv, 0 if negativ 00809 int sign2(float value){ 00810 if(value>=0) 00811 return 1; 00812 else 00813 return 0; 00814 } 00815 00816 //currently line_c is not used 00817 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){ 00818 float lineAngle; 00819 float angleError; 00820 float linear; 00821 float angular; 00822 00823 if(line_b!=0){ 00824 if(direction) 00825 lineAngle=atan(-line_a/line_b); 00826 else 00827 lineAngle=atan(line_a/-line_b); 00828 } 00829 else{ 00830 lineAngle=1.5708; 00831 } 00832 00833 //Computing angle error 00834 angleError = lineAngle-theta; 00835 if(!cos(angleError)) 00836 angleError = atan(sin(angleError)/cos(angleError)); 00837 else 00838 angleError=pi/2; 00839 if(isnan(angleError)) pc.printf("\r\n nan line 794"); 00840 00841 //Calculating velocities 00842 linear=KV*(3.1416); 00843 angular=KH*angleError; 00844 //TODO if we put it like the poly says it fails, if we switch the plus and minus it works ... 00845 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00846 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00847 00848 float aL=*angularLeft; 00849 float aR=*angularRight; 00850 //Normalize speed for motors 00851 if(abs(*angularLeft)>abs(*angularRight)) { 00852 *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR); 00853 *angularLeft=MAX_SPEED*sign1(aL); 00854 } 00855 else { 00856 *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL); 00857 *angularRight=MAX_SPEED*sign1(aR); 00858 } 00859 pc.printf("\r\n line: %f x + %f y + %f =0 , X=%f; Y=%f", line_a, line_b, line_c,robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y()); 00860 } 00861 00862 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c 00863 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){ 00864 /* 00865 in the teachers maths it is 00866 00867 *line_a=forceY; 00868 *line_b=-forceX; 00869 00870 because a*x+b*y+c=0 00871 a impact the vertical and b the horizontal 00872 and he has to put them like this because 00873 Robot space: orthonormal space: 00874 ^ ^ 00875 |x |y 00876 <- R O -> 00877 y x 00878 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 00879 */ 00880 *line_a=forceX; 00881 *line_b=forceY; 00882 //TODO check that 00883 //because the line computed always pass by the robot center we dont need lince_c 00884 //float xRobotOrtho=robot_center_x_in_orthonormal_x(); 00885 //float yRobotOrtho=robot_center_y_in_orthonormal_y(); 00886 //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho; 00887 *line_c=0; 00888 }
Generated on Thu Jul 14 2022 09:22:29 by
1.7.2