Added basic test for location interpolation between known locations
Modified current path prediction to use angles rather than lots of if statements to simplify adding interpolation.
Cleaned up commented out bits of code.
Implemented location interpolation. Also changed location comparison to use an epsilon value rather than strict equality