Diff: Estimation/CartPosition/CartPosition.cpp

Revision:

19:ce7fdade3534

Parent:

2:fbc6e3cf3ed8

--- a/Estimation/CartPosition/CartPosition.cpp	Thu Nov 29 17:21:37 2018 +0000
+++ b/Estimation/CartPosition/CartPosition.cpp	Thu Nov 29 17:26:39 2018 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h" // debug
 #include "CartPosition.h"
-
+#include "util.h"
 #include "globals.h"
 
 CartPosition::CartPosition(void)
@@ -17,30 +17,29 @@
 
 void CartPosition::set(CartPosition p)
 {
-    _x = p._x;
-    _y = p._y;
+    x = p.x;
+    y = p.y;
 }
 
-void CartPosition::set(float x, float y)
+void CartPosition::set(float newx, float newy)
 {
-    _x = x;
-    _y = y;
+    x = newx;
+    y = newy;
 }
 
 
 float CartPosition::bearingTo(CartPosition to)
 {
-    // x and y aren't backwards; it's to correct for the differences between
-    // geometry and navigation. In the former, angles are measured from the x axis,
-    // in the latter, from the y axis.
-    return 180/PI * atan2(to._x-_x, to._y-_y); 
+    float result = clamp360(90 - 180/PI * atan2(to.y-y, to.x-x));
+
+    return result;
 }
 
 
 float CartPosition::distanceTo(CartPosition to)
 {
-    float dx = to._x-_x;
-    float dy = to._y-_y;
+    float dx = to.x-x;
+    float dy = to.y-y;
     
     return sqrt( dx*dx + dy*dy );
 }
@@ -51,8 +50,14 @@
     // geometry and navigation. In the former, angles are measured from the x axis,
     // in the latter, from the y axis.
     float r = bearing * PI / 180;
-    _x += distance * sin( r );
-    _y += distance * cos( r );
+    x += distance * sin( r );
+    y += distance * cos( r );
     
     return;
 }
+
+CartPosition& CartPosition::operator= (CartPosition p)
+{
+    set(p);
+    return *this;
+}