**Odometry and Path Control**

For this competition, each team's robot will demonstrate the ability to measure cumulative translation and rotation using a quadrature encoder to estimate the position (x,y) and rotation θ in a fixed world frame (typically, the starting point of the trajectory, defined to be [0,0,0]).

Teams must implement an interupt handler that responds to rising and falling edges to accumulate positive and negative encoder increments in a cumulative counter for each wheel. The change in the counter value represents a fraction of a wheel rotation that yields the distance traveled Δs(i) for wheel "i" by computing the product of the fractional rotation and the wheel circumference.

The distance traveled for the robot along its current heading is
approximately

Δx = (Δs(left) + Δs(right))/2

the heading change is approximatelyΔθ = atan2( (Δs(right) - Δs(left))/2, WHEEL_BASE/2).

Then the cumulative displacement of the robot in the fixed world coordinate frame is
θ += Δθ,

x += Δx cos(θ), and

y += Δx sin(θ).

Navigation control is designed to translate and rotate as
appropriate to minimize the error between where the robot thinks it
is and where it should be. For example, an error with respect to a
reference orientation could be controlled using a simple PD controller

θ_ddot = -K(θ_act - θ_ref) - B(θ_dot_act)

where θ_ddot is a small acceleration that should be added (aka integrated) into the cumulative PWM command for the right wheel and subtracted from the cumulative PWM command for the left wheel. Considerable tuning and fussing is required to make this work just right.

The same idea can be used for translational control after some careful thought about the geometry of the system. Note that the outcome of the PD controller in this case is an acceleration x_ddot along the robot's current heading and this can implemented directly as a time varying angle offset of the balancing controller (think about it!).

The only remaining issue is to somehow define a time series of reference positions and orientations that implement a sequence of constant curvature path segments. In class, we discussed the "carrot on a stick" model. There are lots of different designs you might consider. Every subsequent challenge problem (i.e. the Precision Challenge and the Maze Challenge) will follow these kinds of path segments; the former being programmer designed and the latter defined by the pattern of Ping sensor feedback you get. Try your best to beat the other teams!