Team 254 - 2014 FRC Code

[artK]

Originally Posted by magnets

I'm not 100% exactly sure how 254's code works, but I'd be willing to bet it uses something similar to what is described in this article. They can just chain the splines together and end up with a continuous and differentiable (smooth) curve because they can set the slope at the beginning and end of each segment equal.

Quintic Hermite Spline Interpolation is indeed what we used, though there was a fair bit of code dedicated to simplifying the coordinate system, so that the x axis was a straight line from waypoint A to waypoint B, and the headings were changed to account for this, though we added the condition that abs(Theta)< PI/2 so the spline can be solved for in our system. We assigned a value d to be the distance along the x axis. Attached is a picture showing the rotation and translation of the coordinate system.

We then solved for an h(x)= ax^5 + bx^4 + .... + f which has the following conditions.
h(0) = 0; h(d) = 0
h'(0) = tan(Theta A); h'(d) = tan(Theta B)
h"(0) = 0; h"(d) = 0
The coefficients then become functions of d, tan(Theta A), and tan(Theta B)

Initially, we ran cubic spline interpolation and saw that is was able to go from A to B smoothly. However, we soon realized that when we went from A to B to C there was some twitching in the robot at B. So to fix this we add the h" terms and set them to zero so the robot would always drive straight at the waypoints.

This was among the funnest piece of code I helped write, and I was super happy to see it impress most of the crowd during the Einstein finals.