Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Estimation/CartPosition/CartPosition.cpp@2:fbc6e3cf3ed8, 2013-06-06 (annotated)

Committer:
shimniok
Date:
Thu Jun 06 13:40:23 2013 +0000
Revision:
2:fbc6e3cf3ed8
Parent:
0:a6a169de725f
Child:
19:ce7fdade3534
Sort-of working version, still some errors with estimation. Added clamp() function.

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 0:a6a169de725f 3
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 0:a6a169de725f 20 _x = p._x;
shimniok 0:a6a169de725f 21 _y = p._y;
shimniok 0:a6a169de725f 22 }
shimniok 0:a6a169de725f 23
shimniok 0:a6a169de725f 24 void CartPosition::set(float x, float y)
shimniok 0:a6a169de725f 25 {
shimniok 0:a6a169de725f 26 _x = x;
shimniok 0:a6a169de725f 27 _y = y;
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 0:a6a169de725f 33 // x and y aren't backwards; it's to correct for the differences between
shimniok 0:a6a169de725f 34 // geometry and navigation. In the former, angles are measured from the x axis,
shimniok 0:a6a169de725f 35 // in the latter, from the y axis.
shimniok 0:a6a169de725f 36 return 180/PI * atan2(to._x-_x, to._y-_y);
shimniok 0:a6a169de725f 37 }
shimniok 0:a6a169de725f 38
shimniok 0:a6a169de725f 39
shimniok 0:a6a169de725f 40 float CartPosition::distanceTo(CartPosition to)
shimniok 0:a6a169de725f 41 {
shimniok 0:a6a169de725f 42 float dx = to._x-_x;
shimniok 0:a6a169de725f 43 float dy = to._y-_y;
shimniok 0:a6a169de725f 44
shimniok 0:a6a169de725f 45 return sqrt( dx*dx + dy*dy );
shimniok 0:a6a169de725f 46 }
shimniok 0:a6a169de725f 47
shimniok 0:a6a169de725f 48 void CartPosition::move(float bearing, float distance)
shimniok 0:a6a169de725f 49 {
shimniok 0:a6a169de725f 50 // x and y aren't backwards; it's to correct for the differences between
shimniok 0:a6a169de725f 51 // geometry and navigation. In the former, angles are measured from the x axis,
shimniok 0:a6a169de725f 52 // in the latter, from the y axis.
shimniok 0:a6a169de725f 53 float r = bearing * PI / 180;
shimniok 0:a6a169de725f 54 _x += distance * sin( r );
shimniok 0:a6a169de725f 55 _y += distance * cos( r );
shimniok 0:a6a169de725f 56
shimniok 0:a6a169de725f 57 return;
shimniok 0:a6a169de725f 58 }