Michael Shimniok / Mbed 2 deprecated DataBus

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/CartPosition/CartPosition.cpp@19:ce7fdade3534, 2018-11-29 (annotated)

Committer:
shimniok
Date:
Thu Nov 29 17:26:39 2018 +0000
Revision:
19:ce7fdade3534
Parent:
2:fbc6e3cf3ed8 
Updated CartPosition, Mapping

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimniok 0:a6a169de725f 1 #include "mbed.h" // debug
shimniok 0:a6a169de725f 2 #include "CartPosition.h"
shimniok 19:ce7fdade3534 3 #include "util.h"
shimniok 2:fbc6e3cf3ed8 4 #include "globals.h"
shimniok 0:a6a169de725f 5
shimniok 0:a6a169de725f 6 CartPosition::CartPosition(void)
shimniok 0:a6a169de725f 7 {
shimniok 0:a6a169de725f 8 set(0,0);
shimniok 0:a6a169de725f 9 }
shimniok 0:a6a169de725f 10
shimniok 0:a6a169de725f 11 CartPosition::CartPosition(float x, float y)
shimniok 0:a6a169de725f 12 {
shimniok 0:a6a169de725f 13 set(x,y);
shimniok 0:a6a169de725f 14 }
shimniok 0:a6a169de725f 15
shimniok 0:a6a169de725f 16 // TODO: 3 fix _x, _y to be private with getters setters
shimniok 0:a6a169de725f 17
shimniok 0:a6a169de725f 18 void CartPosition::set(CartPosition p)
shimniok 0:a6a169de725f 19 {
shimniok 19:ce7fdade3534 20 x = p.x;
shimniok 19:ce7fdade3534 21 y = p.y;
shimniok 0:a6a169de725f 22 }
shimniok 0:a6a169de725f 23
shimniok 19:ce7fdade3534 24 void CartPosition::set(float newx, float newy)
shimniok 0:a6a169de725f 25 {
shimniok 19:ce7fdade3534 26 x = newx;
shimniok 19:ce7fdade3534 27 y = newy;
shimniok 0:a6a169de725f 28 }
shimniok 0:a6a169de725f 29
shimniok 0:a6a169de725f 30
shimniok 0:a6a169de725f 31 float CartPosition::bearingTo(CartPosition to)
shimniok 0:a6a169de725f 32 {
shimniok 19:ce7fdade3534 33 float result = clamp360(90 - 180/PI * atan2(to.y-y, to.x-x));
shimniok 19:ce7fdade3534 34
shimniok 19:ce7fdade3534 35 return result;
shimniok 0:a6a169de725f 36 }
shimniok 0:a6a169de725f 37
shimniok 0:a6a169de725f 38
shimniok 0:a6a169de725f 39 float CartPosition::distanceTo(CartPosition to)
shimniok 0:a6a169de725f 40 {
shimniok 19:ce7fdade3534 41 float dx = to.x-x;
shimniok 19:ce7fdade3534 42 float dy = to.y-y;
shimniok 0:a6a169de725f 43
shimniok 0:a6a169de725f 44 return sqrt( dx*dx + dy*dy );
shimniok 0:a6a169de725f 45 }
shimniok 0:a6a169de725f 46
shimniok 0:a6a169de725f 47 void CartPosition::move(float bearing, float distance)
shimniok 0:a6a169de725f 48 {
shimniok 0:a6a169de725f 49 // x and y aren't backwards; it's to correct for the differences between
shimniok 0:a6a169de725f 50 // geometry and navigation. In the former, angles are measured from the x axis,
shimniok 0:a6a169de725f 51 // in the latter, from the y axis.
shimniok 0:a6a169de725f 52 float r = bearing * PI / 180;
shimniok 19:ce7fdade3534 53 x += distance * sin( r );
shimniok 19:ce7fdade3534 54 y += distance * cos( r );
shimniok 0:a6a169de725f 55
shimniok 0:a6a169de725f 56 return;
shimniok 0:a6a169de725f 57 }
shimniok 19:ce7fdade3534 58
shimniok 19:ce7fdade3534 59 CartPosition& CartPosition::operator= (CartPosition p)
shimniok 19:ce7fdade3534 60 {
shimniok 19:ce7fdade3534 61 set(p);
shimniok 19:ce7fdade3534 62 return *this;
shimniok 19:ce7fdade3534 63 }