Georgios Tsamis
All the lab works are here!
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 //update angularLeft and angularRight 00008 float get_error_angles(float x, float y,float theta); 00009 void test_procedure_Lab_2(); 00010 void procedure_Lab_3(); 00011 void procedure_Lab_2(); 00012 void turn_to_target(float angleToTarget); 00013 void initialise_parameters(); 00014 //fill initialLogValues with the values we already know (here the bordurs) 00015 void fill_initial_log_values(); 00016 //generate a position randomly and makes the robot go there while updating the map 00017 void randomize_and_map(); 00018 //make the robot do a pi/2 flip 00019 void do_half_flip(); 00020 //go the the given position while updating the map 00021 void go_to_point_with_angle(float target_x, float target_y, float target_angle); 00022 void compute_angles_and_distance(float target_x, float target_y, float target_angle); 00023 void compute_linear_angular_velocities(); 00024 //Updates sonar values 00025 void update_sonar_values(float leftMm, float frontMm, float rightMm); 00026 //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] 00027 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected); 00028 //print the map 00029 void print_final_map(); 00030 //print the map with the robot marked on it 00031 void print_final_map_with_robot_position(); 00032 //print the map with the robot and the target marked on it 00033 void print_final_map_with_robot_position_and_target(); 00034 //go to a given line by updating angularLeft and angularRight 00035 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c); 00036 //calculate virtual force field and move 00037 void vff(bool* reached); 00038 void test_got_to_line(bool* reached); 00039 //Go to a given X,Y position, regardless of the final angle 00040 void go_to_point(float target_x, float target_y); 00041 //go to line and follow it, from lab 1 00042 void go_to_line_lab_1(float line_a, float line_b, float line_c); 00043 00044 //MATHS heavy functions 00045 float dist(float robot_x, float robot_y, float target_x, float target_y); 00046 float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c); 00047 //returns the probability [0,1] that the cell is occupied from the log value lt 00048 float log_to_proba(float lt); 00049 //returns the log value that the cell is occupied from the probability value [0,1] 00050 float proba_to_log(float p); 00051 //returns the new log value 00052 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ); 00053 //makes the angle inAngle between 0 and 2pi 00054 float rad_angle_check(float inAngle); 00055 //returns the angle between the vectors (x,y) and (xs,ys) 00056 float compute_angle_between_vectors(float x, float y,float xs,float ys); 00057 float x_robot_in_orthonormal_x(float x, float y); 00058 float y_robot_in_orthonormal_y(float x, float y); 00059 float robot_center_x_in_orthonormal_x(); 00060 float robot_center_y_in_orthonormal_y(); 00061 float robot_sonar_front_x_in_orthonormal_x(); 00062 float robot_sonar_front_y_in_orthonormal_y(); 00063 float robot_sonar_right_x_in_orthonormal_x(); 00064 float robot_sonar_right_y_in_orthonormal_y(); 00065 float robot_sonar_left_x_in_orthonormal_x(); 00066 float robot_sonar_left_y_in_orthonormal_y(); 00067 float estimated_width_indice_in_orthonormal_x(int i); 00068 float estimated_height_indice_in_orthonormal_y(int j); 00069 //update foceX and forceY if necessary 00070 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); 00071 //compute the X and Y force 00072 void compute_forceX_and_forceY(float* forceX, float* forceY); 00073 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c 00074 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c); 00075 //return 1 if positiv, -1 if negativ 00076 float sign1(float value); 00077 //return 1 if positiv, 0 if negativ 00078 int sign2(float value); 00079 //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) 00080 void set_target_to(float x, float y); 00081 void try_to_reach_target(); 00082 void test_map_sonar(); 00083 00084 //those target are in comparaison to the robot (for exemple a robot in 50,50 with a taget of 0,0 would not need to move) 00085 float targetX;//this is in the robot space top left 00086 float targetY;//this is in the robot space top left 00087 //Ortho but for the map (i.e x and Y are > 0) 00088 float targetXOrhto; 00089 float targetYOrhto; 00090 float targetXOrhtoNotMap; 00091 float targetYOrhtoNotMap; 00092 00093 const float pi=3.14159; 00094 00095 //spec of the sonar 00096 //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 00097 const float RANGE_SONAR=50;//cm 00098 const float RANGE_SONAR_MIN=10;//Rmin cm 00099 const float INCERTITUDE_SONAR=10;//cm 00100 const float ANGLE_SONAR=pi/3;//Omega rad 00101 00102 //those distance and angle are approximation in need of measurements, in the orthonormal space 00103 const float ANGLE_FRONT_TO_LEFT=10*pi/36;//50 degrees 00104 const float DISTANCE_SONAR_LEFT_X=-4; 00105 const float DISTANCE_SONAR_LEFT_Y=4; 00106 00107 const float ANGLE_FRONT_TO_RIGHT=-10*pi/36;//-50 degrees 00108 const float DISTANCE_SONAR_RIGHT_X=4; 00109 const float DISTANCE_SONAR_RIGHT_Y=4; 00110 00111 const float ANGLE_FRONT_TO_FRONT=0; 00112 const float DISTANCE_SONAR_FRONT_X=0; 00113 const float DISTANCE_SONAR_FRONT_Y=5; 00114 00115 //TODO adjust the size of the map for computation time (25*25?) 00116 const float WIDTH_ARENA=200;//cm 00117 const float HEIGHT_ARENA=200;//cm 00118 00119 //const int SIZE_MAP=25; 00120 const int NB_CELL_WIDTH=20; 00121 const int NB_CELL_HEIGHT=20; 00122 00123 //used to create the map 250 represent the 250cm of the square where the robot is tested 00124 //float sizeCell=250/(float)SIZE_MAP; 00125 float sizeCellWidth=WIDTH_ARENA/(float)NB_CELL_WIDTH; 00126 float sizeCellHeight=HEIGHT_ARENA/(float)NB_CELL_HEIGHT; 00127 00128 float map[NB_CELL_WIDTH][NB_CELL_HEIGHT];//contains the log values for each cell 00129 float initialLogValues[NB_CELL_WIDTH][NB_CELL_HEIGHT]; 00130 00131 //Diameter of a wheel and distance between the 2 00132 const float RADIUS_WHEELS=3.25; 00133 const float DISTANCE_WHEELS=7.2; 00134 00135 //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space 00136 //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 00137 const float DEFAULT_X=20; 00138 const float DEFAULT_Y=WIDTH_ARENA/2; 00139 //const float DEFAULT_X=20;//lower right 00140 //const float DEFAULT_Y=20;//lower right 00141 const float DEFAULT_THETA=0; 00142 00143 const int MAX_SPEED=50;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP 00144 00145 const float KRHO=12, KA=30, KB=-13, KV=200, KH=200; //Kappa values 00146 00147 //CONSTANT FORCE FIELD 00148 const float FORCE_CONSTANT_REPULSION=10000;//TODO tweak it 00149 const float FORCE_CONSTANT_ATTRACTION=1;//TODO tweak it 00150 const float RANGE_FORCE=30;//TODO tweak it 00151 00152 float alpha; //angle error 00153 float rho; //distance from target 00154 float beta; 00155 float linear, angular, angular_left, angular_right; 00156 float dt=0.5; 00157 float temp; 00158 float d2; 00159 Timer t; 00160 00161 int main(){ 00162 initialise_parameters(); 00163 procedure_Lab_3(); //uses force fields to reach target with set_terget 00164 //procedure_Lab_2(); //picks random targets and makes a map (SUCCESS - builds a more or less accurate map without colliding with obstacles) 00165 00166 //theta=0; 00167 //X=0; 00168 //Y=0; 00169 00170 //go_to_point(16.8, 78.6); //(x,y) in the global coordinates x cm on the x direction, y cm in the y direction form the 0,0 of the table 00171 //(SUCCESS - goes to the specified point) 00172 00173 //go_to_line_lab_1(10, -10, 20); //a,b,c of a line: ax+by+c=0, again on the global coordinates of the table (SUCCESS - goes along the line) 00174 00175 //go_to_point_with_angle(46.8, 78.6, 0);//(x,y,theta) in the global coordinates (SUCCESS - goes to the point, the angle is almost right as well) 00176 00177 //test_procedure_Lab_2(); //starts from the middle of the arena, goes to a point with set_terget 00178 } 00179 00180 void procedure_Lab_3(){ 00181 //try to reach the target is ortho space 00182 set_target_to(0,80);// 00183 print_final_map_with_robot_position_and_target(); 00184 try_to_reach_target(); 00185 //set_target_to(0,20);//lower right 00186 //print_final_map_with_robot_position_and_target(); 00187 //try_to_reach_target(); 00188 //set_target_to(-20,-20);//up left 00189 //print_final_map_with_robot_position_and_target(); 00190 //try_to_reach_target(); 00191 //print the map forever 00192 while(1){ 00193 print_final_map_with_robot_position_and_target(); 00194 } 00195 } 00196 00197 void test_map_sonar(){ 00198 float leftMm; 00199 float frontMm; 00200 float rightMm; 00201 X=20; 00202 Y=WIDTH_ARENA/2; 00203 theta=0; 00204 while(1){ 00205 leftMm = get_distance_left_sensor(); 00206 frontMm = get_distance_front_sensor(); 00207 rightMm = get_distance_right_sensor(); 00208 pc.printf("%f, %f, %f",leftMm,frontMm,rightMm); 00209 //update the probabilities values 00210 update_sonar_values(leftMm, frontMm, rightMm); 00211 print_final_map(); 00212 } 00213 } 00214 00215 void test_procedure_Lab_2(){ 00216 X=HEIGHT_ARENA/2; 00217 Y=WIDTH_ARENA/2; 00218 set_target_to(-100,50); 00219 print_final_map_with_robot_position_and_target(); 00220 go_to_point_with_angle(targetX, targetY, pi/2); 00221 print_final_map_with_robot_position_and_target(); 00222 00223 } 00224 void procedure_Lab_2(){ 00225 for(int i=0;i<15;i++){ 00226 randomize_and_map(); 00227 print_final_map_with_robot_position(); 00228 wait(2); 00229 } 00230 while(1){ 00231 print_final_map_with_robot_position(); 00232 wait(10); 00233 } 00234 } 00235 00236 00237 void try_to_reach_target(){ 00238 //atan2 gives the angle beetween pi and -pi 00239 float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); 00240 turn_to_target(angleToTarget); 00241 bool reached=false; 00242 int print=0; 00243 while (!reached) { 00244 vff(&reached); 00245 //test_got_to_line(&reached); 00246 if(print==10){ 00247 leftMotor(1,0); 00248 rightMotor(1,0); 00249 /* 00250 pc.printf("\r\n theta=%f", theta); 00251 pc.printf("\r\n IN ORTHO:"); 00252 pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x()); 00253 pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y()); 00254 pc.printf("\r\n X Target=%f", targetXOrhto); 00255 pc.printf("\r\n Y Target=%f", targetYOrhto); 00256 */ 00257 print_final_map_with_robot_position_and_target(); 00258 print=0; 00259 }else 00260 print+=1; 00261 } 00262 //Stop at the end 00263 leftMotor(1,0); 00264 rightMotor(1,0); 00265 pc.printf("\r\n target reached"); 00266 wait(3);// 00267 } 00268 00269 //target in ortho space 00270 void set_target_to(float x, float y){ 00271 targetX=y; 00272 targetY=-x; 00273 targetXOrhto=x_robot_in_orthonormal_x(targetX,targetY); 00274 targetYOrhto=y_robot_in_orthonormal_y(targetX,targetY); 00275 targetXOrhtoNotMap=x; 00276 targetYOrhtoNotMap=y; 00277 } 00278 00279 void initialise_parameters(){ 00280 i2c1.frequency(100000); 00281 initRobot(); //Initializing the robot 00282 pc.baud(9600); // baud for the pc communication 00283 00284 measure_always_on();//TODO check if needed 00285 wait(0.5); 00286 //fill the map with the initial log values 00287 fill_initial_log_values(); 00288 00289 theta=DEFAULT_THETA; 00290 X=DEFAULT_X; 00291 Y=DEFAULT_Y; 00292 } 00293 00294 //fill initialLogValues with the values we already know (here the bordurs) 00295 void fill_initial_log_values(){ 00296 //Fill map, we know the border are occupied 00297 for (int i = 0; i<NB_CELL_WIDTH; i++) { 00298 for (int j = 0; j<NB_CELL_HEIGHT; j++) { 00299 if(j==0 || j==NB_CELL_HEIGHT-1 || i==0 || i==NB_CELL_WIDTH-1) 00300 initialLogValues[i][j] = proba_to_log(1); 00301 else 00302 initialLogValues[i][j] = proba_to_log(0.5); 00303 } 00304 } 00305 } 00306 00307 //generate a position randomly and makes the robot go there while updating the map 00308 void randomize_and_map() { 00309 //TODO check that it's aurelien's work 00310 float movementOnX=rand()%(int)(WIDTH_ARENA/2); 00311 float movementOnY=rand()%(int)(HEIGHT_ARENA/2); 00312 00313 float signOfX=rand()%2; 00314 if(signOfX < 1) 00315 signOfX=-1; 00316 float signOfY=rand()%2; 00317 if(signOfY < 1) 00318 signOfY=-1; 00319 00320 float x = movementOnX*signOfX; 00321 float y = movementOnY*signOfY; 00322 float target_angle = 2*((float)(rand()%31416)-15708)/10000.0; 00323 set_target_to(x,y); 00324 go_to_point_with_angle(targetX, targetY, target_angle); 00325 } 00326 00327 00328 void do_half_flip(){ 00329 Odometria(); 00330 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi 00331 if(theta_plus_h_pi > pi) 00332 theta_plus_h_pi=-(2*pi-theta_plus_h_pi); 00333 leftMotor(0,100); 00334 rightMotor(1,100); 00335 while(abs(theta_plus_h_pi-theta)>0.05){ 00336 Odometria(); 00337 // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta)); 00338 } 00339 leftMotor(1,0); 00340 rightMotor(1,0); 00341 } 00342 00343 //ODOMETRIA MUST HAVE BEEN CALLED 00344 //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] 00345 float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected){ 00346 00347 float distancePointToSonar=sqrt(pow(x-xs,2)+pow(y-ys,2)); 00348 if( distancePointToSonar < RANGE_SONAR){ 00349 float anglePointToSonar=compute_angle_between_vectors(x,y,xs,ys);//angle beetween the point and the sonar beam 00350 float alphaBeforeAdjustment=anglePointToSonar-theta-angleFromSonarPosition; 00351 anglePointToSonar=rad_angle_check(alphaBeforeAdjustment);//TODO I feel you don't need to do that but I m not sure 00352 00353 if(alphaBeforeAdjustment>pi) 00354 alphaBeforeAdjustment=alphaBeforeAdjustment-2*pi; 00355 if(alphaBeforeAdjustment<-pi) 00356 alphaBeforeAdjustment=alphaBeforeAdjustment+2*pi; 00357 00358 //float anglePointToSonar2=atan2(y-ys,x-xs)-theta; 00359 00360 //check if the distance between the cell and the robot is within the circle of range RADIUS_WHEELS 00361 //check if absolute difference between the angles is no more than Omega/2 00362 if(anglePointToSonar <= ANGLE_SONAR/2 || anglePointToSonar >= rad_angle_check(-ANGLE_SONAR/2)){ 00363 if( distancePointToSonar < (distanceObstacleDetected - INCERTITUDE_SONAR)){ 00364 //point before obstacle, probably empty 00365 /*****************************************************************************/ 00366 float Ea=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); 00367 float Er; 00368 if(distancePointToSonar < RANGE_SONAR_MIN){ 00369 //point before minimum sonar range 00370 Er=0.f; 00371 }else{ 00372 //point after minimum sonar range 00373 Er=1.f-pow((distancePointToSonar-RANGE_SONAR_MIN)/(distanceObstacleDetected-INCERTITUDE_SONAR-RANGE_SONAR_MIN),2); 00374 } 00375 /*****************************************************************************/ 00376 //if((1.f-Er*Ea)/2.f >1 || (1.f-Er*Ea)/2.f < 0) 00377 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1.f-Er*Ea)/2.f,Er,Ea,alphaBeforeAdjustment); 00378 return (1.f-Er*Ea)/2.f; 00379 }else{ 00380 //probably occupied 00381 /*****************************************************************************/ 00382 float Oa=1.f-pow((2*alphaBeforeAdjustment)/ANGLE_SONAR,2); 00383 float Or; 00384 if( distancePointToSonar <= (distanceObstacleDetected + INCERTITUDE_SONAR)){ 00385 //point between distanceObstacleDetected +- INCERTITUDE_SONAR 00386 Or=1-pow((distancePointToSonar-distanceObstacleDetected)/(INCERTITUDE_SONAR),2); 00387 }else{ 00388 //point after in range of the sonar but after the zone detected 00389 Or=0; 00390 } 00391 /*****************************************************************************/ 00392 //if((1+Or*Oa)/2 >1 || (1+Or*Oa)/2 < 0) 00393 // pc.printf("\n\r return value=%f,Er=%f,Ea=%f,alphaBeforeAdjustment=%f",(1+Or*Oa)/2,Or,Oa,alphaBeforeAdjustment); 00394 return (1+Or*Oa)/2; 00395 } 00396 } 00397 } 00398 //not checked by the sonar 00399 return 0.5; 00400 } 00401 00402 void print_final_map() { 00403 float currProba; 00404 pc.printf("\n\r"); 00405 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00406 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00407 currProba=log_to_proba(map[x][y]); 00408 if ( currProba < 0.5) { 00409 pc.printf(" "); 00410 } else { 00411 if(currProba==0.5) 00412 pc.printf(" . "); 00413 else 00414 pc.printf(" X "); 00415 } 00416 } 00417 pc.printf("\n\r"); 00418 } 00419 } 00420 00421 void print_final_map_with_robot_position() { 00422 float currProba; 00423 Odometria(); 00424 float Xrobot=robot_center_x_in_orthonormal_x(); 00425 float Yrobot=robot_center_y_in_orthonormal_y(); 00426 00427 float heightIndiceInOrthonormal; 00428 float widthIndiceInOrthonormal; 00429 00430 float widthMalus=-(3*sizeCellWidth/2); 00431 float widthBonus=sizeCellWidth/2; 00432 00433 float heightMalus=-(3*sizeCellHeight/2); 00434 float heightBonus=sizeCellHeight/2; 00435 00436 pc.printf("\n\r"); 00437 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00438 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00439 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); 00440 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); 00441 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) 00442 pc.printf(" R "); 00443 else{ 00444 currProba=log_to_proba(map[x][y]); 00445 if ( currProba < 0.5) 00446 pc.printf(" "); 00447 else{ 00448 if(currProba==0.5) 00449 pc.printf(" . "); 00450 else 00451 pc.printf(" X "); 00452 } 00453 } 00454 } 00455 pc.printf("\n\r"); 00456 } 00457 } 00458 00459 void print_final_map_with_robot_position_and_target() { 00460 float currProba; 00461 Odometria(); 00462 float Xrobot=robot_center_x_in_orthonormal_x(); 00463 float Yrobot=robot_center_y_in_orthonormal_y(); 00464 00465 float heightIndiceInOrthonormal; 00466 float widthIndiceInOrthonormal; 00467 00468 float widthMalus=-(3*sizeCellWidth/2); 00469 float widthBonus=sizeCellWidth/2; 00470 00471 float heightMalus=-(3*sizeCellHeight/2); 00472 float heightBonus=sizeCellHeight/2; 00473 00474 pc.printf("\n\r"); 00475 for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { 00476 for (int x= 0; x<NB_CELL_WIDTH; x++) { 00477 heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); 00478 widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); 00479 if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) 00480 pc.printf(" R "); 00481 else{ 00482 if(targetYOrhto >= (heightIndiceInOrthonormal+heightMalus) && targetYOrhto <= (heightIndiceInOrthonormal+heightBonus) && targetXOrhto >= (widthIndiceInOrthonormal+widthMalus) && targetXOrhto <= (widthIndiceInOrthonormal+widthBonus)) 00483 pc.printf(" T "); 00484 else{ 00485 currProba=log_to_proba(map[x][y]); 00486 if ( currProba < 0.5) 00487 pc.printf(" "); 00488 else{ 00489 if(currProba==0.5) 00490 pc.printf(" . "); 00491 else 00492 pc.printf(" X "); 00493 } 00494 } 00495 } 00496 } 00497 pc.printf("\n\r"); 00498 } 00499 } 00500 00501 //MATHS heavy functions 00502 /**********************************************************************/ 00503 //Distance computation function 00504 float dist(float robot_x, float robot_y, float target_x, float target_y){ 00505 //pc.printf("%f, %f, %f, %f",robot_x,robot_y,target_x,target_y); 00506 return sqrt(pow(target_y-robot_y,2) + pow(target_x-robot_x,2)); 00507 } 00508 00509 //returns the probability [0,1] that the cell is occupied from the log valAue lt 00510 float log_to_proba(float lt){ 00511 return 1-1/(1+exp(lt)); 00512 } 00513 00514 //returns the log value that the cell is occupied from the probability value [0,1] 00515 float proba_to_log(float p){ 00516 return log(p/(1-p)); 00517 } 00518 00519 //returns the new log value 00520 float compute_log_estimation_lt(float previousLogValue,float currentProbability,float originalLogvalue ){ 00521 return previousLogValue+proba_to_log(currentProbability)-originalLogvalue; 00522 } 00523 00524 //makes the angle inAngle between 0 and 2pi 00525 float rad_angle_check(float inAngle){ 00526 //cout<<"before :"<<inAngle; 00527 if(inAngle > 0){ 00528 while(inAngle > (2*pi)) 00529 inAngle-=2*pi; 00530 }else{ 00531 while(inAngle < 0) 00532 inAngle+=2*pi; 00533 } 00534 //cout<<" after :"<<inAngle<<endl; 00535 return inAngle; 00536 } 00537 00538 //returns the angle between the vectors (x,y) and (xs,ys) 00539 float compute_angle_between_vectors(float x, float y,float xs,float ys){ 00540 //alpha angle between ->x and ->SA 00541 //vector S to A ->SA 00542 float vSAx=x-xs; 00543 float vSAy=y-ys; 00544 //norme SA 00545 float normeSA=sqrt(pow(vSAx,2)+pow(vSAy,2)); 00546 //vector ->x (1,0) 00547 float cosAlpha=1*vSAy/*+0*vSAx*//normeSA;; 00548 //vector ->y (0,1) 00549 float sinAlpha=/*0*vSAy+*/1*vSAx/normeSA;//+0*vSAx; 00550 if (sinAlpha < 0) 00551 return -acos(cosAlpha); 00552 else 00553 return acos(cosAlpha); 00554 } 00555 00556 //return 1 if positiv, -1 if negativ 00557 float sign1(float value){ 00558 if(value>=0) 00559 return 1; 00560 else 00561 return -1; 00562 } 00563 00564 //return 1 if positiv, 0 if negativ 00565 int sign2(float value){ 00566 if(value>=0) 00567 return 1; 00568 else 00569 return 0; 00570 } 00571 00572 /* 00573 Robot space: orthonormal space: 00574 ^ ^ 00575 |x |y 00576 <- R O -> 00577 y x 00578 */ 00579 //Odometria must bu up to date 00580 float x_robot_in_orthonormal_x(float x, float y){ 00581 return robot_center_x_in_orthonormal_x()-y; 00582 } 00583 00584 //Odometria must bu up to date 00585 float y_robot_in_orthonormal_y(float x, float y){ 00586 return robot_center_y_in_orthonormal_y()+x; 00587 } 00588 00589 float robot_center_x_in_orthonormal_x(){ 00590 return NB_CELL_WIDTH*sizeCellWidth-Y; 00591 } 00592 00593 float robot_center_y_in_orthonormal_y(){ 00594 return X; 00595 } 00596 00597 float robot_sonar_front_x_in_orthonormal_x(){ 00598 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_FRONT_X; 00599 } 00600 float robot_sonar_front_y_in_orthonormal_y(){ 00601 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_FRONT_Y; 00602 } 00603 00604 float robot_sonar_right_x_in_orthonormal_x(){ 00605 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_RIGHT_X; 00606 } 00607 float robot_sonar_right_y_in_orthonormal_y(){ 00608 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_RIGHT_Y; 00609 } 00610 00611 float robot_sonar_left_x_in_orthonormal_x(){ 00612 return robot_center_x_in_orthonormal_x()+DISTANCE_SONAR_LEFT_X; 00613 } 00614 float robot_sonar_left_y_in_orthonormal_y(){ 00615 return robot_center_y_in_orthonormal_y()+DISTANCE_SONAR_LEFT_Y; 00616 } 00617 00618 float estimated_width_indice_in_orthonormal_x(int i){ 00619 return sizeCellWidth/2+i*sizeCellWidth; 00620 } 00621 float estimated_height_indice_in_orthonormal_y(int j){ 00622 return sizeCellHeight/2+j*sizeCellHeight; 00623 } 00624 00625 void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ 00626 //get the coordonate of the map and the robot in the ortonormal space 00627 float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice); 00628 float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice); 00629 //compute the distance beetween the cell and the robot 00630 float distanceCellToRobot=sqrt(pow(xCenterCell-xRobotOrtho,2)+pow(yCenterCell-yRobotOrtho,2)); 00631 //check if the cell is in range 00632 if(distanceCellToRobot <= range) { 00633 /*float anglePointToRobot=compute_angle_between_vectors(xCenterCell,yCenterCell,xRobotOrtho,yRobotOrtho);//angle beetween the point and the sonar 00634 float alphaBeforeAdjustment=anglePointToRobot-theta; 00635 anglePointToRobot=rad_angle_check(alphaBeforeAdjustment); 00636 if(anglePointToRobot <= pi/2 || anglePointToRobot >= rad_angle_check(-pi/2)){ 00637 */ 00638 float probaCell=log_to_proba(map[widthIndice][heightIndice]); 00639 float xForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(xCenterCell-xRobotOrtho)/pow(distanceCellToRobot,3); 00640 float yForceComputed=FORCE_CONSTANT_REPULSION*probaCell*(yCenterCell-yRobotOrtho)/pow(distanceCellToRobot,3); 00641 *forceX+=xForceComputed; 00642 *forceY+=yForceComputed; 00643 //} 00644 } 00645 } 00646 00647 void test_got_to_line(bool* reached){ 00648 float line_a=1; 00649 float line_b=2; 00650 float line_c=-140; 00651 //we update the odometrie 00652 Odometria(); 00653 float angularRight=0; 00654 float angularLeft=0; 00655 00656 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); 00657 //pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); 00658 00659 leftMotor(sign2(angularLeft),abs(angularLeft)); 00660 rightMotor(sign2(angularRight),abs(angularRight)); 00661 00662 pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)); 00663 00664 //wait(0.1); 00665 Odometria(); 00666 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) 00667 *reached=true; 00668 } 00669 void vff(bool* reached){ 00670 float line_a; 00671 float line_b; 00672 float line_c; 00673 //Updating X,Y and theta with the odometry values 00674 float forceX=0; 00675 float forceY=0; 00676 //we update the odometrie 00677 Odometria(); 00678 //we check the sensors 00679 float leftMm = get_distance_left_sensor(); 00680 float frontMm = get_distance_front_sensor(); 00681 float rightMm = get_distance_right_sensor(); 00682 float angularRight=0; 00683 float angularLeft=0; 00684 //update the probabilities values 00685 update_sonar_values(leftMm, frontMm, rightMm); 00686 //we compute the force on X and Y 00687 compute_forceX_and_forceY(&forceX, &forceY); 00688 //we compute a new ine 00689 calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c); 00690 go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); 00691 00692 //Updating motor velocities 00693 00694 leftMotor(sign2(angularLeft),abs(angularLeft)); 00695 rightMotor(sign2(angularRight),abs(angularRight)); 00696 00697 //wait(0.1); 00698 Odometria(); 00699 if(dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)<10) 00700 *reached=true; 00701 } 00702 00703 //robotX and robotY are from Odometria, calculate line_a, line_b and line_c 00704 void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){ 00705 /* 00706 in the teachers maths it is 00707 00708 *line_a=forceY; 00709 *line_b=-forceX; 00710 00711 because a*x+b*y+c=0 00712 a impact the vertical and b the horizontal 00713 and he has to put them like this because 00714 Robot space: orthonormal space: 00715 ^ ^ 00716 |x |y 00717 <- R O -> 00718 y x 00719 but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to 00720 */ 00721 *line_a=forceX; 00722 *line_b=forceY; 00723 //TODO check that 00724 //because the line computed always pass by the robot center we dont need lince_c 00725 //float xRobotOrtho=robot_center_x_in_orthonormal_x(); 00726 //float yRobotOrtho=robot_center_y_in_orthonormal_y(); 00727 //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho; 00728 *line_c=0; 00729 } 00730 00731 /*angleToTarget is obtained through atan2 so it s: 00732 < 0 if the angle is bettween pi and 2pi on a trigo circle 00733 > 0 if it is between 0 and pi 00734 */ 00735 void turn_to_target(float angleToTarget){ 00736 Odometria(); 00737 float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi 00738 if(theta_plus_h_pi > pi) 00739 theta_plus_h_pi=-(2*pi-theta_plus_h_pi); 00740 if(angleToTarget>0){ 00741 leftMotor(0,1); 00742 rightMotor(1,1); 00743 }else{ 00744 leftMotor(1,1); 00745 rightMotor(0,1); 00746 } 00747 while(abs(angleToTarget-theta_plus_h_pi)>0.05){ 00748 Odometria(); 00749 theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi 00750 if(theta_plus_h_pi > pi) 00751 theta_plus_h_pi=-(2*pi-theta_plus_h_pi); 00752 //pc.printf("\n\r diff=%f", abs(angleToTarget-theta_plus_h_pi)); 00753 } 00754 leftMotor(1,0); 00755 rightMotor(1,0); 00756 } 00757 00758 //currently line_c is not used 00759 void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){ 00760 float lineAngle; 00761 float angleError; 00762 float linear; 00763 float angular; 00764 00765 bool direction=true; 00766 00767 float angleToTarget=atan2(targetYOrhtoNotMap,targetXOrhtoNotMap); 00768 bool inFront=true; 00769 if(angleToTarget < 0)//the target is in front 00770 inFront=false; 00771 00772 if(theta > 0){ 00773 //the robot is oriented to the left 00774 if(theta > pi/2){ 00775 //the robot is oriented lower left 00776 if(inFront) 00777 direction=false;//robot and target not oriented the same way 00778 }else{ 00779 //the robot is oriented upper left 00780 if(!inFront) 00781 direction=false;//robot and target not oriented the same way 00782 } 00783 }else{ 00784 //the robot is oriented to the right 00785 if(theta < -pi/2){ 00786 //the robot is oriented lower right 00787 if(inFront) 00788 direction=false;//robot and target not oriented the same way 00789 }else{ 00790 //the robot is oriented upper right 00791 if(!inFront) 00792 direction=false;//robot and target not oriented the same way 00793 } 00794 } 00795 //pc.printf("\r\n Target is in front"); 00796 00797 if(line_b!=0){ 00798 if(!direction) 00799 lineAngle=atan(-line_a/line_b); 00800 else 00801 lineAngle=atan(line_a/-line_b); 00802 } 00803 else{ 00804 lineAngle=1.5708; 00805 } 00806 00807 //Computing angle error 00808 angleError = lineAngle-theta; 00809 angleError = atan(sin(angleError)/cos(angleError)); 00810 00811 //Calculating velocities 00812 linear=KV*(3.1416); 00813 angular=KH*angleError; 00814 00815 *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00816 *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00817 00818 float aL=*angularLeft; 00819 float aR=*angularRight; 00820 //Normalize speed for motors 00821 if(abs(*angularLeft)>abs(*angularRight)) { 00822 *angularRight=MAX_SPEED*abs(aR/aL)*sign1(aR); 00823 *angularLeft=MAX_SPEED*sign1(aL); 00824 } 00825 else { 00826 *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL); 00827 *angularRight=MAX_SPEED*sign1(aR); 00828 } 00829 pc.printf("\r\n X=%f; Y=%f", robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y()); 00830 } 00831 00832 //compute the force on X and Y 00833 void compute_forceX_and_forceY(float* forceX, float* forceY){ 00834 //we put the position of the robot in an orthonormal space 00835 float xRobotOrtho=robot_center_x_in_orthonormal_x(); 00836 float yRobotOrtho=robot_center_y_in_orthonormal_y(); 00837 00838 float forceRepulsionComputedX=0; 00839 float forceRepulsionComputedY=0; 00840 //for each cell of the map we compute a force of repulsion 00841 for(int i=0;i<NB_CELL_WIDTH;i++){ 00842 for(int j=0;j<NB_CELL_HEIGHT;j++){ 00843 update_force(i,j,RANGE_FORCE,&forceRepulsionComputedX,&forceRepulsionComputedY,xRobotOrtho,yRobotOrtho); 00844 } 00845 } 00846 //update with attraction force 00847 *forceX=+forceRepulsionComputedX; 00848 *forceY=+forceRepulsionComputedY; 00849 float distanceTargetRobot=sqrt(pow(targetXOrhto-xRobotOrtho,2)+pow(targetYOrhto-yRobotOrtho,2)); 00850 if(distanceTargetRobot != 0){ 00851 *forceX-=FORCE_CONSTANT_ATTRACTION*(targetXOrhto-xRobotOrtho)/distanceTargetRobot; 00852 *forceY-=FORCE_CONSTANT_ATTRACTION*(targetYOrhto-yRobotOrtho)/distanceTargetRobot; 00853 } 00854 float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); 00855 if(amplitude!=0){//avoid division by 0 if forceX and forceY == 0 00856 *forceX=*forceX/amplitude; 00857 *forceY=*forceY/amplitude; 00858 } 00859 } 00860 00861 //x and y passed are TargetNotMap 00862 float get_error_angles(float x, float y,float theta){ 00863 float angleBeetweenRobotAndTarget=atan2(y,x); 00864 if(y > 0){ 00865 if(angleBeetweenRobotAndTarget < pi/2)//up right 00866 angleBeetweenRobotAndTarget=-pi/2+angleBeetweenRobotAndTarget; 00867 else//up right 00868 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2; 00869 }else{ 00870 if(angleBeetweenRobotAndTarget > -pi/2)//lower right 00871 angleBeetweenRobotAndTarget=angleBeetweenRobotAndTarget-pi/2; 00872 else//lower left 00873 angleBeetweenRobotAndTarget=2*pi+angleBeetweenRobotAndTarget-pi/2; 00874 } 00875 return angleBeetweenRobotAndTarget-theta; 00876 } 00877 00878 void compute_angles_and_distance(float target_x, float target_y, float target_angle){ 00879 alpha = atan2((target_y-Y),(target_x-X))-theta; 00880 alpha = atan(sin(alpha)/cos(alpha)); 00881 rho = dist(X, Y, target_x, target_y); 00882 d2 = rho; 00883 beta = -alpha-theta+target_angle; 00884 00885 //Computing angle error and distance towards the target value 00886 rho += dt*(-KRHO*cos(alpha)*rho); 00887 temp = alpha; 00888 alpha += dt*(KRHO*sin(alpha)-KA*alpha-KB*beta); 00889 beta += dt*(-KRHO*sin(temp)); 00890 //pc.printf("\n\r d2=%f", d2); 00891 //pc.printf("\n\r dt=%f", dt); 00892 } 00893 00894 void compute_linear_angular_velocities(){ 00895 //Computing linear and angular velocities 00896 if(alpha>=-1.5708 && alpha<=1.5708){ 00897 linear=KRHO*rho; 00898 angular=KA*alpha+KB*beta; 00899 } 00900 else{ 00901 linear=-KRHO*rho; 00902 angular=-KA*alpha-KB*beta; 00903 } 00904 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00905 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 00906 00907 //Normalize speed for motors 00908 if(angular_left>angular_right) { 00909 angular_right=MAX_SPEED*angular_right/angular_left; 00910 angular_left=MAX_SPEED; 00911 } else { 00912 angular_left=MAX_SPEED*angular_left/angular_right; 00913 angular_right=MAX_SPEED; 00914 } 00915 } 00916 00917 //go the the given position while updating the map 00918 void go_to_point_with_angle(float target_x, float target_y, float target_angle) { 00919 Odometria(); 00920 alpha = atan2((target_y-Y),(target_x-X))-theta; 00921 alpha = atan(sin(alpha)/cos(alpha)); 00922 rho = dist(X, Y, target_x, target_y); 00923 beta = -alpha-theta+target_angle; 00924 //beta = atan(sin(beta)/cos(beta)); 00925 bool keep_going=true; 00926 float leftMm; 00927 float frontMm; 00928 float rightMm; 00929 do { 00930 //Timer stuff 00931 dt = t.read(); 00932 t.reset(); 00933 t.start(); 00934 00935 //Updating X,Y and theta with the odometry values 00936 Odometria(); 00937 leftMm = get_distance_left_sensor(); 00938 frontMm = get_distance_front_sensor(); 00939 rightMm = get_distance_right_sensor(); 00940 00941 //pc.printf("\n\r leftMm=%f", leftMm); 00942 //pc.printf("\n\r frontMm=%f", frontMm); 00943 //pc.printf("\n\r rightMm=%f", rightMm); 00944 00945 //if in dangerzone 00946 if((frontMm < 150 && frontMm > 0)|| (leftMm <150 && leftMm > 0) || (rightMm <150 && rightMm > 0) ){ 00947 leftMotor(1,0); 00948 rightMotor(1,0); 00949 update_sonar_values(leftMm, frontMm, rightMm); 00950 //TODO Giorgos maybe you can also test the do_half_flip() function 00951 Odometria(); 00952 //do a flip TODO 00953 keep_going=false; 00954 do_half_flip(); 00955 }else{ 00956 //if not in danger zone continue as usual 00957 update_sonar_values(leftMm, frontMm, rightMm); 00958 compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target 00959 compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular) 00960 00961 //pc.printf("\n\r X=%f", X); 00962 //pc.printf("\n\r Y=%f", Y); 00963 00964 //pc.printf("\n\r a_r=%f", angular_right); 00965 //pc.printf("\n\r a_l=%f", angular_left); 00966 00967 //Updating motor velocities 00968 leftMotor(1,angular_left); 00969 rightMotor(1,angular_right); 00970 00971 wait(0.2); 00972 //Timer stuff 00973 t.stop(); 00974 pc.printf("\n\r dist to target= %f",d2); 00975 } 00976 } while(d2>1 && (abs(target_angle-theta)>0.01) && keep_going); 00977 00978 //Stop at the end 00979 leftMotor(1,0); 00980 rightMotor(1,0); 00981 } 00982 00983 //Updates sonar values 00984 void update_sonar_values(float leftMm, float frontMm, float rightMm){ 00985 if(leftMm==0) 00986 pc.printf("\n\r leftMm==0"); 00987 if(frontMm==0) 00988 pc.printf("\n\r frontMm==0"); 00989 if(rightMm==0) 00990 pc.printf("\n\r rightMm==0"); 00991 float currProba; 00992 float i_in_orthonormal; 00993 float j_in_orthonormal; 00994 for(int i=0;i<NB_CELL_WIDTH;i++){ 00995 for(int j=0;j<NB_CELL_HEIGHT;j++){ 00996 //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) 00997 //check that again 00998 //compute for front sonar 00999 i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i); 01000 j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j); 01001 01002 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); 01003 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 01004 //compute for right sonar 01005 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); 01006 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; 01007 //compute for left sonar 01008 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); 01009 map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; 01010 } 01011 } 01012 } 01013 01014 01015 //Go to a given X,Y position, regardless of the final angle 01016 void go_to_point(float target_x, float target_y){ 01017 float angle_error; //angle error 01018 float d; //distance from target 01019 float k_linear=10, k_angular=200; 01020 01021 do { 01022 //Updating X,Y and theta with the odometry values 01023 Odometria(); 01024 01025 //Computing angle error and distance towards the target value 01026 angle_error = atan2((target_y-Y),(target_x-X))-theta; 01027 angle_error = atan(sin(angle_error)/cos(angle_error)); 01028 d=dist(X, Y, target_x, target_y); 01029 01030 //Computing linear and angular velocities 01031 linear=k_linear*d; 01032 angular=k_angular*angle_error; 01033 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 01034 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 01035 01036 01037 //Normalize speed for motors 01038 if(angular_left>angular_right) { 01039 angular_right=MAX_SPEED*angular_right/angular_left; 01040 angular_left=MAX_SPEED; 01041 } else { 01042 angular_left=MAX_SPEED*angular_left/angular_right; 01043 angular_right=MAX_SPEED; 01044 } 01045 01046 pc.printf("\n\r X=%f", X); 01047 pc.printf("\n\r Y=%f", Y); 01048 01049 //Updating motor velocities 01050 leftMotor(1,angular_left); 01051 rightMotor(1,angular_right); 01052 01053 wait(0.5); 01054 } while(d>1); 01055 01056 //Stop at the end 01057 leftMotor(1,0); 01058 rightMotor(1,0); 01059 } 01060 01061 //go to line and follow it, from lab 1 01062 void go_to_line_lab_1(float line_a, float line_b, float line_c){ 01063 01064 float angle_error; //angle error 01065 float d; //distance from line 01066 float kd=5, kh=200, kv=200; 01067 float linear, angular, angular_left, angular_right; 01068 float line_angle; 01069 01070 //Check if b=0, if yes line_angle=90 01071 if(line_b!=0){ 01072 line_angle=atan(-line_a/line_b); 01073 } 01074 else{ 01075 line_angle=1.5708; 01076 } 01077 01078 do { 01079 pc.printf("\n\n\r entered while"); 01080 01081 //Updating X,Y and theta with the odometry values 01082 Odometria(); 01083 01084 //Computing angle error and distance from the target line 01085 angle_error = line_angle-theta; 01086 angle_error = atan(sin(angle_error)/cos(angle_error)); 01087 d=distFromLine(X, Y, line_a, line_b, line_c); 01088 pc.printf("\n\r d=%f", d); 01089 01090 //Calculating velocities 01091 linear=kv*(3.1416-angle_error); 01092 angular=-kd*d+kh*angle_error; 01093 angular_left=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 01094 angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; 01095 01096 //Normalize MAX_SPEED for motors 01097 if(angular_left>angular_right) { 01098 angular_right=MAX_SPEED*angular_right/angular_left; 01099 angular_left=MAX_SPEED; 01100 } 01101 else { 01102 angular_left=MAX_SPEED*angular_left/angular_right; 01103 angular_right=MAX_SPEED; 01104 } 01105 01106 //Updating motor velocities 01107 if(angular_left>0){ 01108 leftMotor(1,angular_left); 01109 } 01110 else{ 01111 leftMotor(0,-angular_left); 01112 } 01113 01114 if(angular_right>0){ 01115 rightMotor(1,angular_right); 01116 } 01117 else{ 01118 rightMotor(0,-angular_right); 01119 } 01120 01121 wait(0.5); 01122 } while(1); 01123 } 01124 01125 float distFromLine(float robot_x, float robot_y, float line_a, float line_b, float line_c){ 01126 return abs((line_a*robot_x+line_b*robot_y+line_c)/sqrt(line_a*line_a+line_b*line_b)); 01127 }
Generated on Mon Aug 8 2022 14:31:09 by
1.7.2