Revision 19:ce7fdade3534, committed 2018-11-29

Comitter:

shimniok

Date:

Thu Nov 29 17:26:39 2018 +0000

Parent:

18:c2f3df4ef5fe

Child:

20:1c2067937065

Commit message:

Updated CartPosition, Mapping

Changed in this revision

--- 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;
+}

--- a/Estimation/CartPosition/CartPosition.h	Thu Nov 29 17:21:37 2018 +0000
+++ b/Estimation/CartPosition/CartPosition.h	Thu Nov 29 17:26:39 2018 +0000
@@ -8,36 +8,47 @@
     /** Create a new cartesian coordinate object
      */
     CartPosition(void);
+
     /** Create a new cartesian coordinate object
      * @param x sets x coordinate
      * @param y sets y coordinate 
      */
     CartPosition(float x, float y);
+
     /** Sets coordinates for object
      * @param x sets x coordinate
      * @param y sets y coordinate 
      */
     void set(float x, float y);
+
+    /** Shorthand for set */
+    CartPosition& operator= (CartPosition p);
+
     /** Sets coordinates for object
      * @param p sets coordinates of this object to that of p
      */
     void set(CartPosition p);
+
     /** Computes bearing to a position from this position
      * @param to is the coordinate to which we're calculating bearing
      */
     float bearingTo(CartPosition to);
+
     /** Computes distance to a position from this position
      * @param to is the coordinate to which we're calculating distance
      */
     float distanceTo(CartPosition to);
+
     /** Computes the new coordinates for this object given a bearing and distance
      * @param bearing is the direction traveled
      * @distance is the distance traveled
      */
     void move(float bearing, float distance);
+
     /** x coordinate of this object */
-    float _x;
+    float x;
+
     /** y coordinate of this object */
-    float _y;
+    float y;
 };
-#endif
\ No newline at end of file
+#endif

--- a/Estimation/Mapping/Mapping.cpp	Thu Nov 29 17:21:37 2018 +0000
+++ b/Estimation/Mapping/Mapping.cpp	Thu Nov 29 17:26:39 2018 +0000
@@ -50,7 +50,7 @@
     lonToX = sw.distanceTo(se) / dlon;
     latToY = sw.distanceTo(nw) / dlat;
 
-    fprintf(stdout, "lonToX=%.10f\nlatToY=%.10f\n", lonToX, latToY);
+    //fprintf(stdout, "lonToX=%.10f\nlatToY=%.10f\n", lonToX, latToY);
 
     return;    
 }
@@ -73,7 +73,7 @@
 
 void Mapping::cartToGeo(CartPosition cart, GeoPosition *pos)
 {
-    cartToGeo(cart._x, cart._y, pos);
+    cartToGeo(cart.x, cart.y, pos);
     
     return;
 }