Diff: source/Movement.cpp

Revision:

120:cdf7a6751f9e

Parent:

117:66d64dbd1b36

Child:

122:8bfb39434fb7

--- a/source/Movement.cpp	Fri May 12 08:33:04 2017 +0000
+++ b/source/Movement.cpp	Fri May 12 15:20:45 2017 +0000
@@ -21,6 +21,8 @@
 Timer t8;                                                                       // timer used for waiting enough distance measurements
 
 int search_state = 0;
+int coord_move_state = 0;
+int list_step = 0;
 
 float left = 0;
 float right = 0;
@@ -252,7 +254,89 @@
  * moves to next coordinate from coordinate list
  * by Claudio Citterio
 **/
+/*
+int move_to_brick_by_list()
+{
+    switch(coord_move_state) {
+        case 0:
+            // init move to brick by list
+            list_step = 1;
 
+            coord_move_state = 1;
+            break;
+        case 1:
+            // get positions, coords, heading and distances
+            float current_heading = get_current_heading();
+            position current_pos = get_current_pos();
+            position next_pos = walkpath[list_step];
+
+            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) {
+                // target reached
+                coord_move_state = 0;
+                return 47;
+            }
+            if(obstacle_list[next_pos.x][next_pos.y] != 0) {
+                // path obstructed
+                coord_move_state = 0;
+                return 35;
+            }
+            coord_move_state = 3;
+            break;
+        case 3:
+            // calc new headings
+            // nord(-y) = 0 grad
+            
+            if(current_pos.y > next_pos.y) {
+                if(current_pos.x > next_pos.x) needed_heading = 315;
+                distance = sqrt2;
+                if(current_pos.x == next_pos.x) needed_heading = 0;
+                distance = 1;
+                if(current_pos.x < next_pos.x) needed_heading = 45;
+                distance = sqrt2;
+            }
+            if(current_pos.y == next_pos.y) {
+                if(current_pos.x > next_pos.x) needed_heading = 270;
+                distance = 1;
+                if(current_pos.x == next_pos.x) //error same position;
+                    if(current_pos.x < next_pos.x) needed_heading = 90;
+                distance = 1;
+            }
+            if(current_pos.y < next_pos.y) {
+                if(current_pos.x > next_pos.x) needed_heading = 225;
+                distance = sqrt2;
+                if(current_pos.x == next_pos.x) needed_heading = 180;
+                distance = 1;
+                if(current_pos.x < next_pos.x) needed_heading = 135;
+                distance = sqrt2;
+            }
+
+            if(needed_heading != current_heading) {
+                coord_move_state = 5;
+            } else {
+                coord_move_state = 8;
+            }
+            break;
+
+        case 5:
+            // turn init with new heading
+            break;
+        case 6:
+            //turn until new heading == heading
+            break;
+
+        case 8:
+            // move init with distance
+            break;
+        case 9:
+            // move until distacne covered
+    }
+}
+*/
 int move_to_next_coord()
 {