Diff: source/Positioning.cpp

Revision:

112:b3270806629f

Parent:

80:92b9d083322d

Child:

118:fbd6d41f6ce8

--- a/source/Positioning.cpp	Mon May 08 07:16:01 2017 +0000
+++ b/source/Positioning.cpp	Mon May 08 12:09:39 2017 +0000
@@ -5,8 +5,8 @@
 
 
 #include "Positioning.h"
-coordinates current_coord;  // Updated
-position current_pos;       // Generated from current_coord
+coordinates current_coord;  // in degree // Updated
+position current_pos;       // in degree // Generated from current_coord
 float current_heading;
 position next_pos;
 
@@ -61,18 +61,35 @@
 void clamp_heading()
 {
 
-    while (direction_l >= 360) direction_l -= 360;
-    while (direction_l < 0) direction_l += 360;
+    while (current_heading >= 360) current_heading -= 360;
+    while (current_heading < 0) current_heading += 360;
 
 }
 
 void positioning()
 {
-    printf("positioning...\r\n");
+    //printf("positioning...\r\n");
+
+    // get wheel_speed
+    float speed_left = get_speed_left();
+    float speed_right = get_speed_right();
+
+    // calc movement and turning speed
+    float movement_speed = (speed_left + speed_right) /2.0f ;
+    float turning_speed = speed_left - speed_right;
+
+    // calc heading and coverd distance
+    current_heading += 360/(2*circle_r*M_PI) * ((2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * turning_speed*0.1f);
+    float distance = (2*(float)wheel_r*(float)M_PI)/(2*M_PI) * t2.read() * movement_speed * 0.1f;
+    clamp_heading();
+    t2.reset();
+    t2.start();
+    
+    // calc new position
+    current_coord.x += sin(current_heading/180.0f*(float)M_PI)* distance;
+    current_coord.y -= cos(current_heading/180.0f*(float)M_PI) * distance;
 
 
-
-    clamp_heading();
 }
 
 
@@ -84,9 +101,9 @@
         last_dist_r = 100;
         last_dist_l = 100;
 
-  //      deg_r = -50;
-    //    deg_l = 50;
-        
+        //      deg_r = -50;
+        //    deg_l = 50;
+
         deg_r = 0;
         deg_l = 0;
 
@@ -99,7 +116,7 @@
     } else {
 
         if(t2 > 0.2f) {
-            
+
             t2.reset();
             float dist_r = getDistanceIR(0);
             float dist_l = getDistanceIR(4);
@@ -108,8 +125,8 @@
             //right
             if(dist_r < last_dist_r) {
                 last_dist_r = dist_r;
-                
-                
+
+
                 deg_r += 5;
                 set_servo_position(2,-deg_r);
             } else if(direction_r == 0) {