Diff: source/Mapping.cpp

Revision:

62:c2fcf3b349e9

Parent:

60:b57577b0072f

Child:

71:ddf4eb5c3081

--- a/source/Mapping.cpp	Tue Apr 18 17:33:22 2017 +0000
+++ b/source/Mapping.cpp	Wed Apr 19 08:34:24 2017 +0000
@@ -31,7 +31,7 @@
 void mapping()
 {
    printf("mapping...\r\n"); 
-    /*
+    
     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) {
@@ -79,7 +79,7 @@
 
         old_pos = current_pos;
         old_heading = current_heading;
-    }*/
+    }
 }
 
 //******************************************************************************
@@ -174,6 +174,40 @@
     return pos;
 }
 
+
+coordinates coordinates_calculation(float distance, float degree, float offsetx, float offsety, float heading, coordinates current_coord)
+{
+    distance *= 100;
+    offsetx *= 100;
+    offsety *= 100;
+
+    coordinates coord = { 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;
+
+    printf("%f || %f || %f || %f\n", x, y, hyp, degree);
+
+    if ((0 <= direction && direction < 90) || (180 <= direction && direction < 270)) {
+        coord.x = current_coord.x + sin(direction / 180 * (float)M_PI)*hyp;
+        coord.y = current_coord.y - cos(direction / 180 * (float)M_PI)*hyp;
+        printf("%f || %f\n", coord.x,coord.y);
+    }
+    if ((90 <= direction && direction < 180) || (270 <= direction && direction < 360)) {
+        coord.x = current_coord.x + sin((180-direction) / 180 * (float)M_PI)*hyp;
+        coord.y = current_coord.y + cos((180-direction) / 180 * (float)M_PI)*hyp;
+        printf("%f || %f\n", coord.x, coord.y);
+    }
+
+    return coord;
+}
 //******************************************************************************
 int select_target()
 {