![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 25:572c9e9a8809
- Parent:
- 24:8f4b820d8de8
- Child:
- 26:b020cf253059
--- a/main.cpp Wed Apr 19 22:20:56 2017 +0000 +++ b/main.cpp Thu Apr 20 15:02:28 2017 +0000 @@ -16,6 +16,8 @@ float compute_probability_t(float x, float y,float xs,float ys, float angleFromSonarPosition, float distanceObstacleDetected); //print the map void print_final_map(); +//print the map with the robot marked on it +void print_final_map_with_robot_position(); //MATHS heavy functions float dist(float robot_x, float robot_y, float target_x, float target_y); @@ -29,6 +31,16 @@ float rad_angle_check(float inAngle); //returns the angle between the vectors (x,y) and (xs,ys) float compute_angle_between_vectors(float x, float y,float xs,float ys); +float robot_center_x_in_orthonormal_x(); +float robot_center_y_in_orthonormal_y(); +float robot_sonar_front_x_in_orthonormal_x(); +float robot_sonar_front_y_in_orthonormal_y(); +float robot_sonar_right_x_in_orthonormal_x(); +float robot_sonar_right_y_in_orthonormal_y(); +float robot_sonar_left_x_in_orthonormal_x(); +float robot_sonar_left_y_in_orthonormal_y(); +float estimated_width_indice_in_orthonormal_x(int i); +float estimated_height_indice_in_orthonormal_y(int j); const float pi=3.14159; //spec of the sonar @@ -272,23 +284,29 @@ void update_sonar_values(float leftMm, float frontMm, float rightMm){ float currProba; + float i_in_orthonormal; + float j_in_orthonormal; 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(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); + i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i); + j_in_orthonormal=estimated_height_indice_in_orthonormal_y(j); + + 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); 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(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); + 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); map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; //compute for left sonar - 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); + 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); map[i][j]=map[i][j]+proba_to_log(currProba)+initialLogValues[i][j]; } } } +//ODOMETRIA MUST HAVE BEEN CALLED //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){ @@ -330,12 +348,12 @@ return (1+Or*Oa)/2; } }else{ - //not in range of the sonar + //not checked by the sonar return 0.5; } } -void print_final_map_1() { +void print_final_map() { float currProba; pc.printf("\n\r"); for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { @@ -354,25 +372,39 @@ } } -void print_final_map_2() { +void print_final_map_with_robot_position() { float currProba; + Odometria(); + float Xrobot=robot_center_x_in_orthonormal_x(); + float Yrobot=robot_center_y_in_orthonormal_y(); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float heightMalus=-(3*sizeCellX/2); + float heightBonus=sizeCellX/2; + + float widthMalus=-(3*sizeCellY/2); + float widthBonus=sizeCellY/2; + 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<= ){ + heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); + widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); + if(Xrobot >= (heightIndiceInOrthonormal+heightMalus) && Xrobot <= (heightIndiceInOrthonormal+heightBonus) && Yrobot >= (widthIndiceInOrthonormal+widthMalus) && Yrobot <= (widthIndiceInOrthonormal+widthBonus)) pc.printf(" R "); - }else{ - if ( currProba < 0.5) { - pc.printf(" 0 "); - } else { - if(currProba==0.5) - pc.printf(" . "); - else - pc.printf(" + "); - } + else{ + currProba=log_to_proba(map[x][y]); + if ( currProba < 0.5) + pc.printf(" 0 "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" + "); + } } - } pc.printf("\n\r"); } @@ -431,4 +463,40 @@ return -acos(cosAlpha); else return acos(cosAlpha); +} + +float robot_center_x_in_orthonormal_x(){ + return Y; +} + +float robot_center_y_in_orthonormal_y(){ + return NB_CELL_WIDTH*sizeCellX-X; +} + +float robot_sonar_front_x_in_orthonormal_x(){ + return Y+DISTANCE_SONAR_FRONT_Y; +} +float robot_sonar_front_y_in_orthonormal_y(){ + return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_FRONT_X; +} + +float robot_sonar_right_x_in_orthonormal_x(){ + return Y+DISTANCE_SONAR_RIGHT_Y; +} +float robot_sonar_right_y_in_orthonormal_y(){ + return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_RIGHT_X; +} + +float robot_sonar_left_x_in_orthonormal_x(){ + return Y+DISTANCE_SONAR_LEFT_Y; +} +float robot_sonar_left_y_in_orthonormal_y(){ + return NB_CELL_WIDTH*sizeCellX-X+DISTANCE_SONAR_LEFT_X; +} + +float estimated_width_indice_in_orthonormal_x(int i){ + return sizeCellX/2+i*sizeCellX; +} +float estimated_height_indice_in_orthonormal_y(int j){ + return sizeCellY/2+j*sizeCellY; } \ No newline at end of file