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/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9a23edd..070cd03 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -144,6 +144,8 @@
class DrivetrainLoop
: public aos::controls::ControlLoop<Goal, Position, Status, Output> {
public:
+ static constexpr size_t kNumSplineFetchers = 4;
+
// Constructs a control loop which can take a Drivetrain or defaults to the
// drivetrain at frc971::control_loops::drivetrain
explicit DrivetrainLoop(const DrivetrainConfig<double> &dt_config,
@@ -154,6 +156,11 @@
virtual ~DrivetrainLoop() {}
protected:
+ struct TrajectoryFetcherState {
+ aos::Fetcher<fb::Trajectory> fetcher;
+ bool in_use = false;
+ };
+
// Executes one cycle of the control loop.
void RunIteration(
const ::frc971::control_loops::drivetrain::Goal *goal,
@@ -165,8 +172,11 @@
flatbuffers::Offset<drivetrain::Output> Zero(
aos::Sender<drivetrain::Output>::Builder *builder) override;
+ void UpdateTrajectoryFetchers();
+
const DrivetrainConfig<double> dt_config_;
DrivetrainFilters filters_;
+ std::array<TrajectoryFetcherState, kNumSplineFetchers> trajectory_fetchers_;
PolyDrivetrain<double> dt_openloop_;
DrivetrainMotorsSS dt_closedloop_;