Diff: source/Movement.cpp

Revision:

122:8bfb39434fb7

Parent:

120:cdf7a6751f9e

Child:

123:de12131ff0b9

diff -r af16ffc05593 -r 8bfb39434fb7 source/Movement.cpp
--- a/source/Movement.cpp	Fri May 12 15:38:01 2017 +0000
+++ b/source/Movement.cpp	Sun May 14 08:35:35 2017 +0000
@@ -13,7 +13,9 @@
 float wanted_deg = 0;
 bool direction = false;
 float restdegAfterstop = 0;                                                              // Variable for Rest degree we still have to cover after e.g. 1 brick found but its not really a brick so we turn further until e.g 60 degrees covered.
-
+float needed_heading = 0;
+float distance_to_next_coord = 0;
+float current_head = 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
 
@@ -254,8 +256,8 @@
  * moves to next coordinate from coordinate list
  * by Claudio Citterio
 **/
-/*
-int move_to_brick_by_list()
+
+int move_to_brick_by_coordlist()
 {
     switch(coord_move_state) {
         case 0:
@@ -266,7 +268,7 @@
             break;
         case 1:
             // get positions, coords, heading and distances
-            float current_heading = get_current_heading();
+            current_head = get_current_heading();
             position current_pos = get_current_pos();
             position next_pos = walkpath[list_step];
 
@@ -290,32 +292,32 @@
         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 = sqrt2;
+                distance_to_next_coord = sqrt2*4;
                 if(current_pos.x == next_pos.x) needed_heading = 0;
-                distance = 1;
+                distance_to_next_coord = 4;
                 if(current_pos.x < next_pos.x) needed_heading = 45;
-                distance = sqrt2;
+                distance_to_next_coord = sqrt2*4;
             }
             if(current_pos.y == next_pos.y) {
                 if(current_pos.x > next_pos.x) needed_heading = 270;
-                distance = 1;
+                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 = 1;
+                distance_to_next_coord = 4;
             }
             if(current_pos.y < next_pos.y) {
                 if(current_pos.x > next_pos.x) needed_heading = 225;
-                distance = sqrt2;
+                distance_to_next_coord = sqrt2*4;
                 if(current_pos.x == next_pos.x) needed_heading = 180;
-                distance = 1;
+                distance_to_next_coord = 4;
                 if(current_pos.x < next_pos.x) needed_heading = 135;
-                distance = sqrt2;
+                distance_to_next_coord = sqrt2*4;
             }
 
-            if(needed_heading != current_heading) {
+            if(needed_heading != current_head) {
                 coord_move_state = 5;
             } else {
                 coord_move_state = 8;
@@ -324,61 +326,34 @@
 
         case 5:
             // turn init with new heading
+            turn_for_deg(needed_heading-current_head,1);
+            coord_move_state = 6;
             break;
         case 6:
             //turn until new heading == heading
+            if(turn_for_deg(0,1)<0) {
+                stop_turn();
+                coord_move_state = 8;
+            }
             break;
 
         case 8:
             // move init with distance
+            move_for_distance(distance_to_next_coord);
+            coord_move_state = 9;
             break;
         case 9:
             // move until distacne covered
-    }
-}
-*/
-int move_to_next_coord()
-{
-
-    float current_heading = get_current_heading();
-    position current_pos = get_current_pos();
-    position next_pos = get_next_pos();
-
-    float needed_heading = 0;
-    float distance = 0;
-
-    // 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(move_for_distance(0)<0) {
+                stop_move();
+                coord_move_state = 0;
+                return 47; //return in switch case creates warning
+            }
+            break;
     }
-    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;
-    }
+    return 45;
+}
 
-    if(needed_heading != current_heading) {
-        turn_for_deg(needed_heading-current_heading,1.0f);
-    } else {
-        move_for_distance(distance);
-    }
-    return 0;
-}
 
 /**
  * this function searchs a nearby brick, moves towards it and grabbs it