Navigate to a given point using the OGM and virtual forces
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 35:c8f224ab153f
- Parent:
- 34:128fc7aed957
- Child:
- 36:c806c568720a
--- a/main.cpp Thu May 04 16:43:04 2017 +0000 +++ b/main.cpp Mon May 15 15:49:26 2017 +0000 @@ -92,8 +92,10 @@ //position and orientation of the robot when put on the map (ODOMETRY doesn't know those) it's in the robot frame //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; +//const float DEFAULT_X=HEIGHT_ARENA/2; +//const float DEFAULT_Y=WIDTH_ARENA/2; +const float DEFAULT_X=10;//lower right +const float DEFAULT_Y=10;//lower right const float DEFAULT_THETA=0; //used to create the map 250 represent the 250cm of the square where the robot is tested @@ -130,8 +132,8 @@ float line_b; float line_c; -float targetX=46.8; -float targetY=78.6; +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 bool reached=false; @@ -147,8 +149,8 @@ fill_initial_log_values(); theta=DEFAULT_THETA; - X=5;//DEFAULT_X; - Y=5;//DEFAULT_Y; + X=DEFAULT_X; + Y=DEFAULT_Y; while (!reached) { vff(); @@ -629,7 +631,7 @@ angular_right=(linear+0.5*DISTANCE_WHEELS*angular)/RADIUS_WHEELS; //Normalize speed for motors - if(angular_left>angular_right) { + if(angular_left>angular_right) {fill_initial_log_values(); angular_right=speed*angular_right/angular_left; angular_left=speed; }