Yeah, we can put something together. We really just wanted to get this out before the summer and haven't quite had the bandwidth to put this together yet.
The waypoint system is for autonomous only. Basically, you can specify a list of waypoints where the start of the path is (0,0,0) in (X,Y,Theta). We calculate a spline that intersects each waypoint at the desired angle. From there we generate a set of (Position, Velocity, Accel) for each wheel at 0.01 second time steps along that spline. This calculation is done off board as the spline formation is pretty cpu hungry. We serialize the data and load it on to the crio as a text file which gets deserialized. The trajectory construction happens in the TrajectoryLib project in
Main.java. The trajectories get deserialized and stored in memory in
AutoPaths.java.
In autonomous, we load a path into the
TrajectoryDriveController and set the Drivebase subsystem's active controller to that. (Each subsystem can have one active controller which controls it).
We do some driver off-load things, but mostly for the operation of the intake and shooter subsystems (for example there is a button that deploys the intake and runs automatically until we sense we've picked up the ball, we only shoot for certain presets once the wheel is settled at speed, etc).