Add a CANDrivetrainWriter
We use this in y2023 and it's likely we'll continue to use it so I
abstracted it out into a seperate file.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Ia8af4f79ab5b0c712d525bc26089b569b9b76d64
diff --git a/frc971/wpilib/can_drivetrain_writer.h b/frc971/wpilib/can_drivetrain_writer.h
new file mode 100644
index 0000000..bd396bf
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.h
@@ -0,0 +1,34 @@
+#include "frc971/can_configuration_generated.h"
+#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+class CANDrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output> {
+ public:
+ CANDrivetrainWriter(::aos::EventLoop *event_loop);
+
+ void set_falcons(std::vector<std::shared_ptr<Falcon>> right_falcons,
+ std::vector<std::shared_ptr<Falcon>> left_falcons);
+
+ void HandleCANConfiguration(const CANConfiguration &configuration);
+
+ static constexpr int kDrivetrainWriterPriority = 35;
+
+ private:
+ void WriteConfigs();
+
+ void Write(
+ const ::frc971::control_loops::drivetrain::Output &output) override;
+
+ void Stop() override;
+
+ std::vector<std::shared_ptr<Falcon>> right_falcons_;
+ std::vector<std::shared_ptr<Falcon>> left_falcons_;
+};
+
+} // namespace wpilib
+} // namespace frc971