Michael Shimniok / Mbed 2 deprecated AVC_2012

Code for autonomous ground vehicle, Data Bus, 3rd place winner in 2012 Sparkfun AVC.

Dependencies:   Watchdog mbed Schedule SimpleFilter LSM303DLM PinDetect DebounceIn Servo

Estimation/CartPosition/CartPosition.cpp@0:826c6171fc1b, 2012-06-20 (annotated)

Committer:
shimniok
Date:
Wed Jun 20 14:57:48 2012 +0000
Revision:
0:826c6171fc1b
Updated documentation

Who changed what in which revision?

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