Diff: source/Positioning.cpp

Revision:

134:5c29654ce301

Parent:

129:0f60bf9640bb

--- a/source/Positioning.cpp	Mon May 15 15:00:20 2017 +0000
+++ b/source/Positioning.cpp	Tue May 16 14:24:11 2017 +0000
@@ -5,8 +5,8 @@
 
 
 #include "Positioning.h"
-coordinates current_coord;  // in degree // Updated
-position current_pos;       // in degree // Generated from current_coord
+coordinates current_coord;  // in cm from 0 point // Updated
+position current_pos;       // in grid (coord / 4)  // Generated from current_coord
 float current_heading;
 position next_pos;
 
@@ -29,16 +29,16 @@
 coordinates pos_to_coord(position pos)
 {
     coordinates coord = {0};
-    coord.x = pos.x + 0.5f;
-    coord.y = pos.y + 0.5f;
+    coord.x = pos.x*4.0f + 2.0f;
+    coord.y = pos.y*4.0f + 2.0f;
     return coord;
 }
 
 position coord_to_pos(coordinates coord)
 {
     position pos = {0};
-    pos.x = rint(coord.x/4.0f - 0.5f);    //round values
-    pos.y = rint(coord.y/4.0f - 0.5f);
+    pos.x = rint(coord.x/4.0f - 2.0f);    //round values
+    pos.y = rint(coord.y/4.0f - 2.0f);
     return pos;
 }
 
@@ -71,25 +71,32 @@
     //printf("positioning...\r\n");
 
     // get wheel_speed
-    float speed_left = get_speed_left();
-    float speed_right = get_speed_right();
+    float speed_left = fabsf(get_speed_left());
+    float speed_right = fabsf(get_speed_right());
+
+    //printf("\r\nspeeds: %f || %f\r\n", speed_left, speed_right);
 
     // calc movement and turning speed
-    float movement_speed = (speed_left + speed_right) /2.0f ;
-    float turning_speed = speed_left - speed_right;
+    float movement_speed = (speed_left + speed_right) / 2.0f ;
+    float turning_speed = (speed_left - speed_right) / 2.0f ;
+    
+    printf("\nmovement_speed %f\r\n", movement_speed);
+    printf("turning_speed %f\r\n",turning_speed);
+    printf("time: %f\r\n\n",t2.read());
 
     // 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;
 
-     current_pos = coord_to_pos(current_coord);
+    current_pos = coord_to_pos(current_coord);
 }
 
 
@@ -129,7 +136,7 @@
 
                 deg_r += 3;
                 set_servo_position(2,deg_r);
-                
+
             } else if(current_coord.y == 0) {
                 deg_r -= 5; //compensate overturning
                 //deg_r -= 3; // compensate not straight looking
@@ -149,7 +156,7 @@
                 last_dist_l = dist_l;
                 deg_l -= 5;
                 set_servo_position(0,deg_l);
-                
+
             } else if(current_coord.x == 0) {
                 deg_l += 3; //compensate overturning
                 deg_l -= 3; //compensate not straight looking
@@ -164,14 +171,14 @@
             }
 
             if(current_coord.x != 0 && current_coord.y != 0) {
-                
+
                 printf("degs: %d || %d \r\n", deg_l , deg_r);
                 printf("coords : %f || %f \r\n", current_coord.x, current_coord.y);
                 current_heading = (360-deg_r + 270-deg_l)/2;
 
                 clamp_heading();
                 printf("heading: %f \r\n", current_heading);
-                
+
                 current_coord.x = 20;
                 current_coord.y = 20;