Diff: source/Mapping.cpp

Revision:

50:2e2bf0815fd9

Parent:

49:652438112348

Child:

51:4a18b47fd659

diff -r 652438112348 -r 2e2bf0815fd9 source/Mapping.cpp
--- a/source/Mapping.cpp	Thu Apr 13 22:45:22 2017 +0000
+++ b/source/Mapping.cpp	Sat Apr 15 00:11:16 2017 +0000
@@ -26,9 +26,9 @@
 //******************************************************************************
 void mapping()
 {
-    position new_pos = get_current_pos();
-    float new_heading = get_current_heading();
-    if(old_heading != new_heading || old_pos.x != new_pos.x || old_pos.y != new_pos.y) {
+    position current_pos = get_current_pos();
+    float current_heading = get_current_heading();
+    if(old_heading != current_heading || old_pos.x != current_pos.x || old_pos.y != current_pos.y) {
         //check_sensor(left);
         float distance0 = getDistanceIR(0);
         if(distance0 < 0.75f && distance0 > 0.1f) {
@@ -40,7 +40,7 @@
             } else {
                 object = 1;
             }
-            position mapping_pos = position_calculation(distance0, 50, 0.12, 0.12, new_heading);
+            position mapping_pos = position_calculation(distance0, 50, 0.12, 0.12, current_heading, current_pos);
             draw_to_map(mapping_pos, object);
         }
         //check_sensor(right);
@@ -53,7 +53,7 @@
             } else {
                 object = 1;
             }
-            position mapping_pos = position_calculation(distance4, -50, -0.12, 0.12, new_heading);
+            position mapping_pos = position_calculation(distance4, -50, -0.12, 0.12, current_heading, current_pos);
             draw_to_map(mapping_pos, object);
         }
         //check_sensor(center);
@@ -66,12 +66,12 @@
             } else {
                 object = 1;
             }
-            position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, new_heading);
+            position mapping_pos = position_calculation(distance2, 0, -0.12, 0.12, current_heading, current_pos);
             draw_to_map(mapping_pos, object);
         }
 
-        old_pos = new_pos;
-        old_heading = new_heading;
+        old_pos = current_pos;
+        old_heading = current_heading;
     }
 }
 
@@ -132,11 +132,37 @@
 }
 
 //******************************************************************************
-position position_calculation(float distance,float degree,float offsetx,float offsety, float heading)
+position position_calculation(float distance, float degree, float offsetx, float offsety, float heading, position current_pos)
 {
+    distance *= 100;
+    offsetx *= 100;
+    offsety *= 100;
+
+    position pos = { 0 };
+    float direction = 0;
+
+    float x = (offsetx + sin(degree/180*(float)M_PI)*distance)/4;
+    float y = (-offsety - cos(degree/180*(float)M_PI)*distance)/4;
+    float hyp = sqrt(x*x+y*y);
+    direction = asin(x/hyp)/(float)M_PI*180;
+    direction += heading;
 
+    while (direction >= 360) direction -= 360;
+    while (direction < 0) direction += 360;
 
-    position pos = {0};
+    printf("%f || %f || %f || %f\n", x, y, hyp, degree);
+
+    if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
+        pos.x = current_pos.x + rint(sin(direction / 180 * (float)M_PI)*hyp);
+        pos.y = current_pos.y - rint(cos(direction / 180 * (float)M_PI)*hyp);
+        printf("%d || %d\n", pos.x,pos.y);
+    }
+    if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
+        pos.x = current_pos.x + rint(sin((180-direction) / 180 * (float)M_PI)*hyp);
+        pos.y = current_pos.y + rint(cos((180-direction) / 180 * (float)M_PI)*hyp);
+        printf("%d || %d\n", pos.x, pos.y);
+    }
+
     return pos;
 }