Diff: source/Movement.cpp

Revision:

52:56399c2f13cd

Parent:

39:92723f7ea54f

Child:

53:453f24775644

--- a/source/Movement.cpp	Sat Apr 15 00:18:50 2017 +0000
+++ b/source/Movement.cpp	Tue Apr 18 11:50:02 2017 +0000
@@ -5,41 +5,72 @@
 
 #include "Movement.h"
 
-int moving(){
-    
+int moving()
+{
+
     return 0;
-    }
-    
-int moving_forward_for_distance(){
-    
+}
+
+int move_forward_for_distance(float distance)
+{
+
     return 0;
-    }
-    
-int moving_backward_for_distance(){
-    
+}
+
+int move_backward_for_distance()
+{
+
     return 0;
-    }
-    
-int turn_left_for_deg(){
-    
+}
+
+int turn_for_deg(float deg)
+{
+
     return 0;
-    }
-    
-int turn_right_for_deg(){
+}
+
+
+int move_to_next_coord()
+{
+
+    float current_heading = get_current_heading();
+    position current_position = get_current_pos();
+    position next_pos = get_next_pos();
     
-    return 0;
-    }
-    
-int move_to_next_coord(){
+    float needed_heading = 0;
+    float distance = 0;
     
-    return 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(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) {
+        turn_for_deg(needed_heading-current_heading);
+    } else {  
+        move_forard_for_distance(distance);
     }
-    
-int move_in_search_for_brick(){
-    
     return 0;
-    }
-    
+}
+
+int move_in_search_for_brick()
+{
+
+    return 0;
+}
+
 
 
 
@@ -103,7 +134,7 @@
     enableMotorDriver = 1;
 
     PID_correction_value = 1.0f;
-    
+
 }
 // ******************************************************************************
 void move_forward_slow(float correction_value)
@@ -145,7 +176,7 @@
 void sync_movement(bool speed, bool direction)
 {
  if(counterLeft.read() > 30000 || -counterRight > 30000){
-     
+
      }
  printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
     if(counterLeft.read() > -counterRight.read()) {
@@ -154,7 +185,7 @@
         if(counterLeft.read() < -counterRight.read()) {
             PID_correction_value -= 0.001f;
         } else {
-            // even 
+            // even
         }
     }