[UNTESTED, DO NOT MERGE YET] Path Follower

[tropicaaal]

This is a port of taolib's path follower to JAR. The implementation has been roughly ported from this function.

This isn't pure pursuit per se, but rather a somewhat simplified version of it that uses PID targeting for waypoint intersections. The algorithm has been fairly effective at following curves nonetheless. Arc curvature of the path and whatnot is not considered, and the path is followed at constant velocity (determined by lookahead_distance). The higher the lookahead, the faster but less accurate it will follow the path.

Example (make a robot follow y=6sin(x/6)):

#include <vector>

drive.follow_path({Point(0.000000, 0.000000),Point(0.992082, 0.987568),Point(1.984164, 1.948197),Point(2.976246, 2.855684),Point(3.968328, 3.685276),Point(4.960409, 4.414343),Point(5.952491, 5.022999),Point(6.944573, 5.494640),Point(7.936655, 5.816402),Point(8.928737, 5.979507),Point(9.920819, 5.979507),Point(10.912901, 5.816402),Point(11.904983, 5.494640),Point(12.897065, 5.022999),Point(13.889146, 4.414343),Point(14.881228, 3.685276),Point(15.873310, 2.855684),Point(16.865392, 1.948197),Point(17.857474, 0.987568),Point(18.849556, 0.000000});

This path was generated using the following python script:

import math

import numpy as np

points = []
formatted = "{"

for i in np.linspace(0, 6 * math.pi, 20):
  points.append((i, 6*math.sin(i/6)));

for p in points:
  formatted += "Point(%f, %f)," %(p[0], p[1])

formatted = formatted[:-2]
formatted += "}"

print(formatted)

Some notes:

• To make 2D point calculations less of a headache, i've added a Point struct that stores x and y values. This means that Odom no longer deals with separate float values for x and y. I could go a lot further with this and overload drive_to_point and other functions in Drive to support passing around Point and also clean up the math used in other functions, but for now it's just used as a helper for pursuit math-related things.
• I haven't added overloaded versions of follow_path for customizing things like max voltage and PID gains per-movement yet, but let me know what should be added in that area.
• The actual generation for the curves is left up to the user. The path follower can in theory follow any list of 2d points smoothly.