Diff: source/Movement.cpp

Revision:

132:8ae08f41bb43

Parent:

123:de12131ff0b9

Child:

134:5c29654ce301

--- a/source/Movement.cpp	Mon May 15 13:03:16 2017 +0000
+++ b/source/Movement.cpp	Mon May 15 14:58:27 2017 +0000
@@ -156,7 +156,7 @@
 **/
 float move_for_distance(float distance)
 {
-    printf("move for distance\r\n");
+    //printf("move for distance\r\n");
     if(distance != 0) {
 
         is_moving = true;
@@ -177,6 +177,7 @@
         devider = true;
         t.reset();
         t.start();
+        printf("move for %f m\r\n", distance);
 
     } else {
         float speed_multiplier = 0.6f;
@@ -190,7 +191,7 @@
         }
 
         float speed_left = get_speed_left();
-        printf("speed left: %f\r\n", speed_left);
+        //printf("speed left: %f\r\n", speed_left);
         wanted_dist -= (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t.read() * fabsf(speed_left)*0.1f;
         t.reset();
 
@@ -200,7 +201,7 @@
             t.stop();
         }
     }
-    printf("remaining distance to cover: %f\r\n", wanted_dist);
+    //printf("remaining distance to cover: %f\r\n", wanted_dist);
     return wanted_dist;
 }
 
@@ -231,6 +232,7 @@
         devider = true;
         t.reset();
         t.start();
+        printf("turn %f deg\r\n", deg);
 
     } else {
         float speed_multiplier = 0.6f;
@@ -250,7 +252,7 @@
             t.stop();
         }
     }
-    printf("remaining deg %f\r\n", wanted_deg);
+    //printf("remaining deg %f\r\n", wanted_deg);
     return (wanted_deg);
 }
 
@@ -273,20 +275,22 @@
             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);
             next_coord = pos_to_coord(next_position);
-
+            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:
             // check if path is still possible with updated map or target reached
 
             if(next_position.x == 0 && next_position.y == 0) {
-                // target reached
+                printf("target reached\r\n");
                 coord_move_state = 0;
                 return 47;
             }
             if(obstacle_list[next_position.x][next_position.y] != 0) {
-                // path obstructed
+                printf("path obstructed\r\n");
                 coord_move_state = 0;
                 return 35;
             }
@@ -294,14 +298,15 @@
             coord_move_state = 3;
             break;
         case 3:
-            // calc new headings
-            float x = next_coord.x - current_coordinates.x;
-            float y = next_coord.y - current_coordinates.y;
+            // 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;
             distance_to_next_coord = sqrt(x*x + y*y);
             
             needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;
             if (x < 0) needed_heading += 180;
-
+            printf("current heading %f\r\n", current_head);
+            printf("needed heading %f\r\n", needed_heading);
             if(needed_heading != current_head) {
                 coord_move_state = 5;
             } else {
@@ -311,7 +316,9 @@
 
         case 5:
             // turn init with new heading
-            turn_for_deg(needed_heading-current_head,1);
+            float turn_deg = needed_heading-current_head;
+            if( turn_deg > 180.0f) turn_deg -= 360.0f;
+            turn_for_deg(turn_deg,1);
             coord_move_state = 6;
             break;
         case 6: