Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 39:dd8326ec75ce
- Parent:
- 38:3c9f8cbf5250
- Child:
- 40:f5e212d9f900
diff -r 3c9f8cbf5250 -r dd8326ec75ce main.cpp --- a/main.cpp Thu May 18 13:44:31 2017 +0000 +++ b/main.cpp Thu May 18 15:06:25 2017 +0000 @@ -4,6 +4,7 @@ using namespace std; +void initialise_parameters(); //fill initialLogValues with the values we already know (here the bordurs) void fill_initial_log_values(); //generate a position randomly and makes the robot go there while updating the map @@ -20,6 +21,8 @@ void print_final_map(); //print the map with the robot marked on it void print_final_map_with_robot_position(); +//print the map with the robot and the target marked on it +void print_final_map_with_robot_position_and_target(); //go to a given line void go_to_line(); //calculate virtual force field and move @@ -37,6 +40,8 @@ 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 x_robot_in_orthonormal_x(float x, float y); +float y_robot_in_orthonormal_y(float x, float y); float robot_center_x_in_orthonormal_x(); float robot_center_y_in_orthonormal_y(); float robot_sonar_front_x_in_orthonormal_x(); @@ -52,7 +57,7 @@ //update foceX and forceY if necessary void updateForce(int widthIndice, int heightIndice, float range, float* forceX, float* forceY, float xRobotOrtho, float yRobotOrtho ); //compute the X and Y force -void compute_forceX_and_forceY(float targetX, float targetY,float* forceX, float* forceY); +void compute_forceX_and_forceY(float* forceX, float* forceY); const float pi=3.14159; @@ -110,12 +115,14 @@ const float RADIUS_WHEELS=3.25; const float DISTANCE_WHEELS=7.2; -const int MAX_SPEED=100;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP +const int MAX_SPEED=200;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP + +//TODO all those global variables are making me sad float alpha; //angle error float rho; //distance from target float beta; -float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values //TODO check kv, kh for go_to_line +float kRho=12, ka=30, kb=-13, kv=200, kh=200; //Kappa values float linear, angular, angular_left, angular_right; float dt=0.5; float temp; @@ -132,38 +139,49 @@ float line_b; float line_c; -float targetX=HEIGHT_ARENA-10;//this is in the robot frame top left -float targetY=WIDTH_ARENA-10;//this is in the robot frame top left +//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) +const float targetX=HEIGHT_ARENA*2/3;//this is in the robot frame top left +const float targetY=WIDTH_ARENA*2/3;//this is in the robot frame top left +float targetX_orhto=0; +float targetY_orhto=0; bool reached=false; int main(){ + initialise_parameters(); + + //try to reach the target + while (!reached) { + vff(); + print_final_map_with_robot_position_and_target(); + } + + //Stop at the end + leftMotor(1,0); + rightMotor(1,0); + //print the map + while(1){ + print_final_map_with_robot_position_and_target(); + } + +} + +void initialise_parameters(){ i2c1.frequency(100000); initRobot(); //Initializing the robot pc.baud(9600); // baud for the pc communication measure_always_on();//TODO check if needed wait(0.5); - + //fill the map with the initial log values fill_initial_log_values(); theta=DEFAULT_THETA; X=DEFAULT_X; Y=DEFAULT_Y; - - while (!reached) { - vff(); - print_final_map_with_robot_position(); - } - - //Stop at the end - leftMotor(1,0); - rightMotor(1,0); - - while(1){ - print_final_map_with_robot_position(); - } + targetX_orhto=x_robot_in_orthonormal_x(targetX,targetY); + targetY_orhto=y_robot_in_orthonormal_y(targetX,targetY); } //fill initialLogValues with the values we already know (here the bordurs) @@ -179,6 +197,8 @@ } } + + //generate a position randomly and makes the robot go there while updating the map void randomize_and_map() { //TODO check that it's aurelien's work @@ -402,6 +422,48 @@ } } +void print_final_map_with_robot_position_and_target() { + float currProba; + Odometria(); + float Xrobot=robot_center_x_in_orthonormal_x(); + float Yrobot=robot_center_y_in_orthonormal_y(); + + float heightIndiceInOrthonormal; + float widthIndiceInOrthonormal; + + float widthMalus=-(3*sizeCellWidth/2); + float widthBonus=sizeCellWidth/2; + + float heightMalus=-(3*sizeCellHeight/2); + float heightBonus=sizeCellHeight/2; + + pc.printf("\n\r"); + for (int y = NB_CELL_HEIGHT -1; y>-1; y--) { + for (int x= 0; x<NB_CELL_WIDTH; x++) { + heightIndiceInOrthonormal=estimated_height_indice_in_orthonormal_y(y); + widthIndiceInOrthonormal=estimated_width_indice_in_orthonormal_x(x); + if(Yrobot >= (heightIndiceInOrthonormal+heightMalus) && Yrobot <= (heightIndiceInOrthonormal+heightBonus) && Xrobot >= (widthIndiceInOrthonormal+widthMalus) && Xrobot <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" R "); + else{ + if(targetY_orhto >= (heightIndiceInOrthonormal+heightMalus) && targetY_orhto <= (heightIndiceInOrthonormal+heightBonus) && targetX_orhto >= (widthIndiceInOrthonormal+widthMalus) && targetX_orhto <= (widthIndiceInOrthonormal+widthBonus)) + pc.printf(" T "); + else{ + currProba=log_to_proba(map[x][y]); + if ( currProba < 0.5) + pc.printf(" "); + else{ + if(currProba==0.5) + pc.printf(" . "); + else + pc.printf(" X "); + } + } + } + } + pc.printf("\n\r"); + } +} + //MATHS heavy functions /**********************************************************************/ //Distance computation function @@ -573,13 +635,11 @@ } //compute the force on X and Y -void compute_forceX_and_forceY(float targetX, float targetY, float* forceX, float* forceY){ +void compute_forceX_and_forceY(float* forceX, float* forceY){ //we put the position of the robot in an orthonormal frame float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); - //we put the target of the robot in an orthonormal frame - float targetXOrtho=x_robot_in_orthonormal_x(targetX,targetY); - float targetYOrtho=y_robot_in_orthonormal_y(targetX,targetY); + float forceRepulsionComputedX=0; float forceRepulsionComputedY=0; //for each cell of the map we compute a force of repulsion @@ -591,8 +651,8 @@ //update with attraction force *forceX=-forceRepulsionComputedX; *forceY=-forceRepulsionComputedY; - *forceX+=FORCE_CONSTANT_ATTRACTION*(targetXOrtho-xRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); - *forceY+=FORCE_CONSTANT_ATTRACTION*(targetYOrtho-yRobotOrtho)/sqrt(pow(targetXOrtho-xRobotOrtho,2)+pow(targetYOrtho-yRobotOrtho,2)); + *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX_orhto-xRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); + *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY_orhto-yRobotOrtho)/sqrt(pow(targetX_orhto-xRobotOrtho,2)+pow(targetY_orhto-yRobotOrtho,2)); float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); if(amplitude!=0){//avoid division by 0 *forceX=*forceX/amplitude; @@ -600,6 +660,7 @@ } } +//robotX and robotY are from Odometria void calculate_line(float forceX, float forceY, float robotX, float robotY){ line_a=forceY; line_b=-forceX; @@ -619,7 +680,7 @@ //update the probabilities values update_sonar_values(leftMm, frontMm, rightMm); //we compute the force on X and Y - compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY); + compute_forceX_and_forceY(&forceX, &forceY); //we compute a new ine calculate_line(forceX, forceY, X, Y); go_to_line();