Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 44:e2bd06f94dc0
- Parent:
- 43:ffd5a6d4dd48
- Child:
- 45:fb07065a64a9
diff -r ffd5a6d4dd48 -r e2bd06f94dc0 main.cpp --- a/main.cpp Mon May 22 18:20:08 2017 +0000 +++ b/main.cpp Mon May 29 11:52:56 2017 +0000 @@ -68,17 +68,18 @@ //return 1 if positiv, 0 if negativ int sign2(float value); void set_target_to(float x, float y); +void try_to_reach_target(); 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 +//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 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 -//those distance and angle are approximation in need of measurements, in the orthonormal frame +//those distance and angle are approximation in need of measurements, in the orthonormal space 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; @@ -99,7 +100,7 @@ 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) it's in the robot frame +//position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot space //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 //const float DEFAULT_X=HEIGHT_ARENA/2; //const float DEFAULT_Y=WIDTH_ARENA/2; @@ -130,30 +131,53 @@ const float RANGE_FORCE=50;//TODO tweak it //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) -float targetX;//this is in the robot frame top left -float targetY;//this is in the robot frame top left +float targetX;//this is in the robot space top left +float targetY;//this is in the robot space top left float targetXOrhto; float targetYOrhto; int main(){ - initialise_parameters(); - - //try to reach the target - bool reached=false; + initialise_parameters(); + //try to reach the target + set_target_to(50,0);//up right + print_final_map_with_robot_position_and_target(); + try_to_reach_target(); + set_target_to(-50,0);//lower right + print_final_map_with_robot_position_and_target(); + try_to_reach_target(); + set_target_to(50,50);//up left + print_final_map_with_robot_position_and_target(); + try_to_reach_target(); + //print the map forever + while(1){ + print_final_map_with_robot_position_and_target(); + } +} + +void try_to_reach_target(){ + bool reached=false; + int print=0; while (!reached) { vff(&reached); //test_got_to_line(&reached); - print_final_map_with_robot_position_and_target(); + if(print==10){ + leftMotor(1,0); + rightMotor(1,0); + pc.printf("\r\n theta=%f", theta); + pc.printf("\r\n IN ORTHO:"); + pc.printf("\r\n X Robot=%f", robot_center_x_in_orthonormal_x()); + pc.printf("\r\n Y Robot=%f", robot_center_y_in_orthonormal_y()); + pc.printf("\r\n X Target=%f", targetXOrhto); + pc.printf("\r\n Y Target=%f", targetYOrhto); + print_final_map_with_robot_position_and_target(); + print=0; + }else + print+=1; } //Stop at the end leftMotor(1,0); rightMotor(1,0); - - //print the map forever - while(1){ - print_final_map_with_robot_position_and_target(); - } - + pc.printf("\r\n target reached"); } void set_target_to(float x, float y){ @@ -176,7 +200,6 @@ theta=DEFAULT_THETA; X=DEFAULT_X; Y=DEFAULT_Y; - set_target_to(HEIGHT_ARENA-40,WIDTH_ARENA-40);//50 cm up and 80 on the right } //fill initialLogValues with the values we already know (here the bordurs) @@ -297,7 +320,7 @@ 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 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) //check that again //compute for front sonar i_in_orthonormal=estimated_width_indice_in_orthonormal_x(i); @@ -517,7 +540,7 @@ /* -Robot frame: orthonormal frame: +Robot space: orthonormal space: ^ ^ |x |y <- R O -> @@ -617,7 +640,7 @@ } void update_force(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ){ - //get the coordonate of the map and the robot in the ortonormal frame + //get the coordonate of the map and the robot in the ortonormal space float xCenterCell=estimated_width_indice_in_orthonormal_x(widthIndice); float yCenterCell=estimated_height_indice_in_orthonormal_y(heightIndice); //compute the distance beetween the cell and the robot @@ -634,7 +657,7 @@ //compute the force on X and Y void compute_forceX_and_forceY(float* forceX, float* forceY){ - //we put the position of the robot in an orthonormal frame + //we put the position of the robot in an orthonormal space float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); @@ -661,16 +684,6 @@ } } -//robotX and robotY are from Odometria, calculate line_a, line_b and line_c -void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){ - *line_a=forceY; - *line_b=-forceX; - //TODO check in what referentiel it needs to go - float xRobotOrtho=robot_center_x_in_orthonormal_x(); - float yRobotOrtho=robot_center_y_in_orthonormal_y(); - *line_c=forceX*yRobotOrtho-forceY*xRobotOrtho; -} - void test_got_to_line(bool* reached){ float line_a=1; float line_b=2; @@ -686,8 +699,6 @@ leftMotor(sign2(angularLeft),abs(angularLeft)); rightMotor(sign2(angularRight),abs(angularRight)); - pc.printf("\r\n angR=%f", angularRight); - pc.printf("\r\n angL=%f", angularLeft); pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)); //wait(0.1); @@ -717,19 +728,11 @@ //we compute a new ine calculate_line(forceX, forceY, X, Y,&line_a,&line_b,&line_c); go_to_line(&angularLeft,&angularRight,line_a,line_b,line_c); - - pc.printf("\r\n forceX=%f", forceX); - pc.printf("\r\n forceY=%f", forceY); - pc.printf("\r\n line: %f x + %f y + %f =0", line_a, line_b, line_c); //Updating motor velocities leftMotor(sign2(angularLeft),abs(angularLeft)); rightMotor(sign2(angularRight),abs(angularRight)); - - pc.printf("\r\n angR=%f", angularRight); - pc.printf("\r\n angL=%f", angularLeft); - pc.printf("\r\n dist=%f", dist(robot_center_x_in_orthonormal_x(),robot_center_y_in_orthonormal_y(),targetXOrhto,targetYOrhto)); //wait(0.1); Odometria(); @@ -737,6 +740,23 @@ *reached=true; } +//return 1 if positiv, -1 if negativ +float sign1(float value){ + if(value>=0) + return 1; + else + return -1; +} + +//return 1 if positiv, 0 if negativ +int sign2(float value){ + if(value>=0) + return 1; + else + return 0; +} + +//currently line_c is not used void go_to_line(float* angularLeft,float* angularRight,float line_a, float line_b, float line_c){ float lineAngle; float angleError; @@ -757,8 +777,8 @@ linear=KV*(3.1416); angular=KH*angleError; //TODO if we put it like the poly says it fails, if we switch the plus and minus it works ... - *angularLeft=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; - *angularRight=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; + *angularLeft=(linear-0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; + *angularRight=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; float aL=*angularLeft; float aR=*angularRight; @@ -771,27 +791,33 @@ *angularLeft=MAX_SPEED*abs(aL/aR)*sign1(aL); *angularRight=MAX_SPEED*sign1(aR); } - - pc.printf("\r\n theta=%f", theta); - pc.printf("\r\n angleError=%f", angleError); - pc.printf("\r\n X=%f", robot_center_x_in_orthonormal_x()); - pc.printf("\r\n Y=%f", robot_center_y_in_orthonormal_y()); - pc.printf("\r\n TX=%f", targetXOrhto); - pc.printf("\r\n TY=%f", targetYOrhto); + 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()); } -//return 1 if positiv, -1 if negativ -float sign1(float value){ - if(value>=0) - return 1; - else - return -1; -} - -//return 1 if positiv, 0 if negativ -int sign2(float value){ - if(value>=0) - return 1; - else - return 0; +//robotX and robotY are from Odometria, calculate line_a, line_b and line_c +void calculate_line(float forceX, float forceY, float robotX, float robotY,float *line_a, float *line_b, float *line_c){ + /* + in the teachers maths it is + + *line_a=forceY; + *line_b=-forceX; + + because a*x+b*y+c=0 + a impact the vertical and b the horizontal + and he has to put them like this because + Robot space: orthonormal space: + ^ ^ + |x |y + <- R O -> + y x + but since our forceX, forceY are already computed in the orthonormal space I m not sure we need to + */ + *line_a=forceX; + *line_b=forceY; + //TODO check that + //because the line computed always pass by the robot center we dont need lince_c + //float xRobotOrtho=robot_center_x_in_orthonormal_x(); + //float yRobotOrtho=robot_center_y_in_orthonormal_y(); + //*line_c=forceX*yRobotOrtho+forceY*xRobotOrtho; + *line_c=0; } \ No newline at end of file