Diff: source/Movement.cpp

Revision:

134:5c29654ce301

Parent:

132:8ae08f41bb43

Child:

135:f31d24150f5e

--- a/source/Movement.cpp	Mon May 15 15:00:20 2017 +0000
+++ b/source/Movement.cpp	Tue May 16 14:24:11 2017 +0000
@@ -270,21 +270,24 @@
 
             coord_move_state = 1;
             break;
+            
+            
         case 1:
             // get positions, coords, heading and distances
             current_head = get_current_heading();
             current_coordinates = get_current_coord();
             position next_position = walkpath[list_step];
-            printf("next position: %d || %d\r\n",  next_position.x, next_position.y);
+            position current_pos = get_current_pos();
             next_coord = pos_to_coord(next_position);
+            printf("\r\nlist step: %d\r\n", list_step);
+            printf("current coordinates: %f || %f\r\n", current_coordinates.x, current_coordinates.y);
             printf("next coordinate: %f || %f\r\n",  next_coord.x, next_coord.y);
-            printf("list step: %d\r\n", list_step);
-            coord_move_state = 2;
-            break;
-        case 2:
+            printf("current position: %d || %d\r\n", current_pos.x, current_pos.y);
+            printf("next position: %d || %d\r\n",  next_position.x, next_position.y);
+                        
             // check if path is still possible with updated map or target reached
-
-            if(next_position.x == 0 && next_position.y == 0) {
+            printf("checking\r\n");
+            if(next_position.x < 1.0f && next_position.y < 1.0f) {
                 printf("target reached\r\n");
                 coord_move_state = 0;
                 return 47;
@@ -297,10 +300,12 @@
             list_step += 1;
             coord_move_state = 3;
             break;
+            
+            
         case 3:
             // calc new heading and distance
-            float x = (next_coord.x - current_coordinates.x) / 25.0f;
-            float y = (next_coord.y - current_coordinates.y) / 25.0f;
+            float x = (next_coord.x - current_coordinates.x) / 100.0f; // distance to cover in x direction [in m]
+            float y = (next_coord.y - current_coordinates.y) / 100.0f; // distance to cover in y direction [in m]
             distance_to_next_coord = sqrt(x*x + y*y);
             
             needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;