Odometry

To determine the correct position of the robot on the field, odometry is used with the tracking wheel's encoder values.

The full version of the odometry code is available on the "src" folder of "LeftAuto" in my Github repository.

Odometry Derivation

The left image is the robot.

circle = center of the robot

rectangle = tracking wheel with encoder

Let SR = distance from the robot’s center to the right tracking wheel

Let SL = distance from the robot’s center to the left tracking wheel

Let SB = distance from the robot’s center to the back tracking wheel

Assume that the robot traveled in this arc and let 𝛉 be the angle formed at the center of the circle. Let r be the radius of the circle. Let 𝚫L be the arc length travelled by the left tracking wheel and 𝚫R be the arc length travelled by the right one.

Then, 𝚫L = (r + SL)𝛉 and 𝚫R = (r - SR)𝛉

𝚫L/𝛉 = r + SL ⇒ r = 𝚫L/𝛉 -SL

𝚫R/𝛉 = r - SL ⇒ r = 𝚫R/𝛉 +SR

𝚫L/𝛉 -SL = 𝚫R/𝛉 +SR

𝚫L -SL𝛉 = 𝚫R + SR𝛉 ⇒ 𝚫L - 𝚫R = 𝛉(SL+SR)

𝛉 = (𝚫L - 𝚫R)/(SL+SR)

Use cos law:

y2 = r2 + r2 - 2r*r*cos𝛉 =2r2(1-cos𝛉)

y2 = 4r2((1-cos𝛉)/2) ⇒ y = 2r(sqrt(1-cos𝛉)/2)

Since sin(𝛉/2) = sqrt((1-cos𝛉)/2), y = 2r(sin(𝛉/2))

Since r = 𝚫R/𝛉 +SR,

y = 2(𝚫R/𝛉 +SR)*sin(𝛉/2)

We can apply this theory for x axis in terms of backtracking wheel as well.

Let 𝚫B be the arc travelled by the backtracking wheel.

x = 2(𝚫B/𝛉+SL)*sin(𝛉/2)

So there we go. If we know the distances travelled by the tracking wheels and the heading by inertial sensor, we can track our position for the robot.

Odometry Code

This code is based upon the math derivation that I showed above. Although the logic for deltaR described in the section above is used for deltaR in the code, it doesn't change what it does. The right tracking wheel's encoder value had to be reversed because the ports were reversed in configuration. The "positionTracking" task not only calculates the X, Y coordinates on the field, but it also prints the X Y values on the controller screen and the brain screen with a live diagram of the robot's movement on the field. For the values of X Y printed on the controller, they are not exact because the values are updated every 50 ticks of the encoders.