Explanation of Mathematical

Components

Determining an absolute position (while

stopped)
Using the Law of Sines:

y
A

And:
180
= 90

8 sin
sin

8 sin
sin

+ 180

and set equal:

sin
sin
sin
sin
= 270

Solve for

c
8

sin

sin

270

cos

cos 270

sin ( ) (Trig Identity)

sin
sin
sin (

+ cos" 270

+ $%$&2

sin
sin

+ cos 270

+ sin" 270

, sin 270

(Trig identity)
Then = 180& $%$&2 (

)
(where n is an integer)
And = 270

+ cos 270
( )

, sin 270

Determining an absolute position (while

Finally,
stopped)
sin 180
8

x
C

c
8

* = 8 8 sin

+ = 8 8 sin

sin 180 ( )
sin 180

sin 180

Determining an absolute angle (while

stopped)
A

Once the absolute position is known, then the

robots angle () with respect to east is:

B
C

;=

+ 90 180

;=

;=

90 + +

+ +

90

Determining an absolute angle (while

stopped)
A

B
C

This formula obtains the position given the

NW, NE, and SE beacons. In order to obtain our
position using any 3 of the four beacons, we
can apply a rotation matrix to the original
*, +, ;:
*
* cos ;

+
+ sin ;

sin ;
cos ;

And then add a constant shift based on which

beacons were used:
*LMNO%
*
*
+ + + +LMNO%
;
;
;LMNO%
Specific shifts for beacon combinations can be
found in PhotoTransistor_Service.h

In order to enable the bot to determine its position while moving, we

must account for the fact that the bot will have moved on the field
between angular measurements to different beacons

Southeast
beacon

Northeast
beacon

Northwest
beacon

Problem: Given the linear distance traveled by each of the two wheels (L1
and L2), and the distance between the two wheels (d) can we determine the
displacement (x, y) of the robot?

(x, y)
L1

L2

ST
R=
T" T

L1 = (r + d)
L2 = (r)

T" T
;=
S

L1

L2

d
r+d

ST
R=
T" T
T" T
;=
S

S
;
=2 U+
sin
2
2
ST
S
T" T
"
"
* ++ =2
+
sin
T" T
2
2S
S(T +T" )
T" T
"
"
* ++ =
sin
T" T
2S

Distance is the base of

an isosceles triangle:

*"

r+d

+ +"

Under the assumption that the bot

followed an arc of constant radius,
the angle () that the displacement
vector makes with a reference
direction is the average of starting
() and ending () angles from the
same reference
This assumption should be valid, as
the motor setpoints should not
change until position has been
updated (which will occur at the end
of all calculations)

Updating bearing between measurements

Thus, to obtain our new bearing V from our old bearing V:
180 ;
V = V + 2 90
2
T" T
X
V =V+
S
X

Formula for updating position between

measurements
So:
S(T +T" )
T" T
sin
T" T
2S
S(T +T" )
T" T
+ =
sin
T" T
2S

* =

Z+[
2
Z+[
sin
2

cos

Where:
d is distance between wheels, L1 and L2 are linear distance traveled by the two wheels, Z
and [ are the starting and finishing angles with respect to east
*Note: This formula does not apply when L1 = L2. However, for this case, the calculation
of distance is trivial.
*Note: Do not use this formula if either L1 or L2 are negative (rotation, not translation)

Error Handling
Errors will compound over time, and at times we will need to stop
and do a stationary sweep in order to correct error on our absolute
position incurred while moving
However, if we are accurate enough, it may suffice to correct our
error at each capture station by setting the known coordinates as our
absolute position (offset by the distance to the particular hall sensor
that was firing)

Rotation
The angle of rotation can be measured by encoder ticks. The wheel
travels along an arc of length \, equal to R;, where R is half of the
distance between the wheels.
Thus,
%N]`L _M^^\ ]NR]abO^R^&]^
SNL%$&]^ ^%_^^& _M^^\L
;=
\=
ca\L^L
2
SRNd^ e^$R R$%Nf
R^d
ca\L^L
SNL%$&]^ ^%_^^& _M^^\L ; SRNd^ e^$R R$%Nf
R^d
%N]`L =
2 _M^^\ ]NR]abO^R^&]^