Refactor trajectory generation to separate process
This pulls all the trajectory planning into a TrajectoryGenerator class,
which produces a Trajectory spline that the drivetrain code can consume
and use to track the spline.
Broadly speaking, this change:
- Separates the Trajectory class into a generation class and a
FinishedTrajectory class, where the generator produces a flatbuffer
and the FinishedTrajectory reads all the required information from
the flatbuffer.
- Add an option for serialization/deserialization of a DistanceSpline.
- Removes some dead code from Trajectory class (mostly having to do with
the old feedback algorithm).
- Uses floats in more places, to keep the size of the Trajectory
flatbuffer under control
- Update the tests & autonomous code to use the new spline code.
Further work that may make sense:
- Experiment with alternatives to current structure of the Trajectory
flatbuffer to see whether (a) the size is a problem; and (b) if so,
what we should do about it.
- Add shims to allow replaying logfiles with old-style spline goals.
Change-Id: Ic80ce4e384ec4d1bd22940580e3652ecd305b352
diff --git a/frc971/control_loops/drivetrain/trajectory.fbs b/frc971/control_loops/drivetrain/trajectory.fbs
new file mode 100644
index 0000000..ab14238
--- /dev/null
+++ b/frc971/control_loops/drivetrain/trajectory.fbs
@@ -0,0 +1,44 @@
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.drivetrain.fb;
+
+enum SegmentConstraint : byte {
+ VELOCITY_LIMITED,
+ CURVATURE_LIMITED,
+ ACCELERATION_LIMITED,
+ DECELERATION_LIMITED,
+ VOLTAGE_LIMITED,
+}
+
+table PlanPoint {
+ // Distance along the path of this point.
+ distance: float (id: 0);
+ velocity: float (id: 1);
+ segment_constraint: SegmentConstraint (id: 2);
+}
+
+table GainPoint {
+ distance: float (id: 0);
+ // Column-major matrix (should be 2 rows x 5 columns).
+ gains: [float] (id: 1);
+}
+
+table DistanceSpline {
+ spline:frc971.MultiSpline (id: 0);
+ distances:[float] (id: 1);
+}
+
+table Trajectory {
+ // Unique ID of the trajectory, same as that defined in the SplineGoal.
+ handle:int (id: 0);
+ // Spline plan, indexed on distance.
+ distance_based_plan : [PlanPoint] (id : 1);
+ // Gains, indexed on time.
+ gains: [GainPoint] (id: 2);
+ spline:DistanceSpline (id: 3);
+
+ // Whether to follow the spline driving forwards or backwards.
+ drive_spline_backwards:bool (id: 4);
+}
+
+root_type Trajectory;