Diff: source/Movement.cpp

Revision:

123:de12131ff0b9

Parent:

122:8bfb39434fb7

Child:

131:8ae08f41bb43

--- a/source/Movement.cpp	Sun May 14 08:35:35 2017 +0000
+++ b/source/Movement.cpp	Sun May 14 09:34:37 2017 +0000
@@ -16,6 +16,8 @@
 float needed_heading = 0;
 float distance_to_next_coord = 0;
 float current_head = 0;
+coordinates current_coordinates = {0};
+coordinates next_coord = {0};
 
 float TOLERANCE_BRICK_OR_OBSTACLE = 0.08f;                                       // Variable for Brick detection it sets how much upper sensor and lower can differ that its still detected as an obstacle and not a brick
 
@@ -269,60 +271,43 @@
         case 1:
             // get positions, coords, heading and distances
             current_head = get_current_heading();
-            position current_pos = get_current_pos();
-            position next_pos = walkpath[list_step];
+            current_coordinates = get_current_coord();
+            position next_position = walkpath[list_step];
+            next_coord = pos_to_coord(next_position);
 
             coord_move_state = 2;
             break;
         case 2:
             // check if path is still possible with updated map or target reached
 
-            if(next_pos.x == 0 && next_pos.y == 0) {
+            if(next_position.x == 0 && next_position.y == 0) {
                 // target reached
                 coord_move_state = 0;
                 return 47;
             }
-            if(obstacle_list[next_pos.x][next_pos.y] != 0) {
+            if(obstacle_list[next_position.x][next_position.y] != 0) {
                 // path obstructed
                 coord_move_state = 0;
                 return 35;
             }
+            list_step += 1;
             coord_move_state = 3;
             break;
         case 3:
             // calc new headings
-            // nord(-y) = 0 grad
-            // non static would be better
-            if(current_pos.y > next_pos.y) {
-                if(current_pos.x > next_pos.x) needed_heading = 315;
-                distance_to_next_coord = sqrt2*4;
-                if(current_pos.x == next_pos.x) needed_heading = 0;
-                distance_to_next_coord = 4;
-                if(current_pos.x < next_pos.x) needed_heading = 45;
-                distance_to_next_coord = sqrt2*4;
-            }
-            if(current_pos.y == next_pos.y) {
-                if(current_pos.x > next_pos.x) needed_heading = 270;
-                distance_to_next_coord = 4;
-                if(current_pos.x == next_pos.x) //error same position;
-                    if(current_pos.x < next_pos.x) needed_heading = 90;
-                distance_to_next_coord = 4;
-            }
-            if(current_pos.y < next_pos.y) {
-                if(current_pos.x > next_pos.x) needed_heading = 225;
-                distance_to_next_coord = sqrt2*4;
-                if(current_pos.x == next_pos.x) needed_heading = 180;
-                distance_to_next_coord = 4;
-                if(current_pos.x < next_pos.x) needed_heading = 135;
-                distance_to_next_coord = sqrt2*4;
-            }
+            float x = next_coord.x - current_coordinates.x;
+            float y = next_coord.y - current_coordinates.y;
+            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;
 
             if(needed_heading != current_head) {
                 coord_move_state = 5;
             } else {
                 coord_move_state = 8;
             }
-            break;
+            break;          
 
         case 5:
             // turn init with new heading
@@ -343,11 +328,10 @@
             coord_move_state = 9;
             break;
         case 9:
-            // move until distacne covered
+            // move until distance is covered
             if(move_for_distance(0)<0) {
                 stop_move();
-                coord_move_state = 0;
-                return 47; //return in switch case creates warning
+                coord_move_state = 1;
             }
             break;
     }