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;
     }