![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 34:128fc7aed957
- Parent:
- 33:78139f82ea74
- Child:
- 35:c8f224ab153f
diff -r 78139f82ea74 -r 128fc7aed957 main.cpp --- a/main.cpp Thu May 04 15:21:24 2017 +0000 +++ b/main.cpp Thu May 04 16:43:04 2017 +0000 @@ -2,7 +2,7 @@ #include "robot.h" // Initializes the robot. This include should be used in all main.cpp! #include "math.h" - using namespace std; +using namespace std; //fill initialLogValues with the values we already know (here the bordurs) void fill_initial_log_values(); @@ -52,13 +52,13 @@ //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 targetX, float targetY,float* forceX, float* forceY); const float pi=3.14159; //CONSTANT FORCE FIELD -const float FORCE_CONSTANT_REPULSION=10;//TODO tweak it +const float FORCE_CONSTANT_REPULSION=50;//TODO tweak it const float FORCE_CONSTANT_ATTRACTION=10;//TODO tweak it const float RANGE_FORCE=50;//TODO tweak it @@ -108,7 +108,7 @@ const float RADIUS_WHEELS=3.25; const float DISTANCE_WHEELS=7.2; -const int MAX_SPEED=250;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP +const int MAX_SPEED=100;//TODO TWEAK THE SPEED SO IT DOES NOT FUCK UP float alpha; //angle error float rho; //distance from target @@ -130,8 +130,8 @@ float line_b; float line_c; -float targetX; -float targetY; +float targetX=46.8; +float targetY=78.6; bool reached=false; @@ -147,8 +147,8 @@ fill_initial_log_values(); theta=DEFAULT_THETA; - X=DEFAULT_X; - Y=DEFAULT_Y; + X=5;//DEFAULT_X; + Y=5;//DEFAULT_Y; while (!reached) { vff(); @@ -552,20 +552,22 @@ } } -void compute_forceX_and_forceY(float targetX, float targetY,float forceX, float forceY){ +void compute_forceX_and_forceY(float targetX, float targetY,float* forceX, float* forceY){ float xRobotOrtho=robot_center_x_in_orthonormal_x(); float yRobotOrtho=robot_center_y_in_orthonormal_y(); for(int i=0;i<NB_CELL_WIDTH;i++){ for(int j=0;j<NB_CELL_HEIGHT;j++){ - updateForce(i,j,RANGE_FORCE,&forceX,&forceY,xRobotOrtho,yRobotOrtho); + updateForce(i,j,RANGE_FORCE,forceX,forceY,xRobotOrtho,yRobotOrtho); } } - //update with attraction force - forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-xRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2)); - forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-yRobotOrtho)/sqrt(pow(targetX-xRobotOrtho,2)+pow(targetY-yRobotOrtho,2)); - float amplitude=sqrt(pow(forceX,2)+pow(forceY,2)); - forceX=forceX/amplitude; - forceY=forceY/amplitude; + //update with attraction force + *forceX=-*forceX; + *forceY=-*forceY; + *forceX+=FORCE_CONSTANT_ATTRACTION*(targetX-X)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2)); + *forceY+=FORCE_CONSTANT_ATTRACTION*(targetY-Y)/sqrt(pow(targetX-X,2)+pow(targetY-Y,2)); + float amplitude=sqrt(pow(*forceX,2)+pow(*forceY,2)); + *forceX=*forceX/amplitude; + *forceY=*forceY/amplitude; } void calculate_line(float forceX, float forceY, float robotX, float robotY){ @@ -584,17 +586,26 @@ rightMm = get_distance_right_sensor(); update_sonar_values(leftMm, frontMm, rightMm); - updateForce(WIDTH_ARENA, HEIGHT_ARENA, 20, &forceX, &forceY, X, Y); //TODO check range value, I randomly put 20 - compute_forceX_and_forceY(targetX, targetY,forceX, forceY); + updateForce(WIDTH_ARENA, HEIGHT_ARENA, RANGE_FORCE, &forceX, &forceY, robot_center_x_in_orthonormal_x(), robot_center_y_in_orthonormal_y()); //TODO check range value, I randomly put 20 + compute_forceX_and_forceY(targetX, targetY,&forceX, &forceY); calculate_line(forceX, forceY, X, Y); + go_to_line(); + 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(1,angular_left); rightMotor(1,angular_right); + + pc.printf("\r\n angR=%f", angular_right); + pc.printf("\r\n angL=%f", angular_left); + pc.printf("\r\n dist=%f", dist(X,Y,targetX,targetY)); - wait(0.2); + + wait(0.1); Odometria(); - if(dist(X,Y,targetX,targetY)<0.5) + if(dist(X,Y,targetX,targetY)<10) reached=true; }