Aesl Program
|
Odometry (angle, x, y), updated at 100Hz. More...
Variables | |
integer[1] | odo.delta |
[out]instantaneous speed difference | |
integer[1] | odo.theta = {0} |
[out] odometer current angle | |
integer[1] | odo.x = {0} |
[out] odometer x | |
integer[1] | odo.y = {0} |
[out] odometer y | |
integer[1] | odo.degree |
[out] odometer direction | |
Odometry (angle, x, y), updated at 100Hz.
A simple dead reckoning is calculed from the two motor target speeds \( M_R \) and \( M_L \). Since we are updating at 100 Hz, the angles will be small and we can use the formulas from (Borenstein et al 1996) as described by (Lucas 2000):
\[ \Delta = (M_R + M_L)/2 \]
\[ \theta \,+\!\!= k_1 (M_R - M_L)/b \]
\[ x \,+\!\!= k_2 \Delta \cos\theta \]
\[ y \,+\!\!= k_2 \Delta \sin\theta \]
where \( b=95 \) mm is the wheelbase, and constants \( k_1=32.36 \), \( k_2=6.78\times10^{-7} \) scale to and from Aseba radians.