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/BUILD b/frc971/wpilib/BUILD
index 33a3cd1..7ce36e4 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -518,6 +518,23 @@
)
cc_library(
+ name = "can_drivetrain_writer",
+ srcs = [
+ "can_drivetrain_writer.cc",
+ ],
+ hdrs = [
+ "can_drivetrain_writer.h",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:roborio"],
+ deps = [
+ ":falcon",
+ ":loop_output_handler",
+ "//frc971:can_configuration_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_output_fbs",
+ ],
+)
+
+cc_library(
name = "can_sensor_reader",
srcs = ["can_sensor_reader.cc"],
hdrs = ["can_sensor_reader.h"],
diff --git a/frc971/wpilib/can_drivetrain_writer.cc b/frc971/wpilib/can_drivetrain_writer.cc
new file mode 100644
index 0000000..bdff308
--- /dev/null
+++ b/frc971/wpilib/can_drivetrain_writer.cc
@@ -0,0 +1,66 @@
+#include "frc971/wpilib/can_drivetrain_writer.h"
+
+using frc971::wpilib::CANDrivetrainWriter;
+
+CANDrivetrainWriter::CANDrivetrainWriter(::aos::EventLoop *event_loop)
+ : ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::drivetrain::Output>(event_loop,
+ "/drivetrain") {
+ event_loop->SetRuntimeRealtimePriority(kDrivetrainWriterPriority);
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+}
+
+void CANDrivetrainWriter::set_falcons(
+ std::vector<std::shared_ptr<Falcon>> right_falcons,
+ std::vector<std::shared_ptr<Falcon>> left_falcons) {
+ right_falcons_ = std::move(right_falcons);
+ left_falcons_ = std::move(left_falcons);
+}
+
+void CANDrivetrainWriter::HandleCANConfiguration(
+ const CANConfiguration &configuration) {
+ for (auto falcon : right_falcons_) {
+ falcon->PrintConfigs();
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->PrintConfigs();
+ }
+
+ if (configuration.reapply()) {
+ WriteConfigs();
+ }
+}
+
+void CANDrivetrainWriter::WriteConfigs() {
+ for (auto falcon : right_falcons_) {
+ falcon->WriteConfigs();
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteConfigs();
+ }
+}
+
+void CANDrivetrainWriter::Write(
+ const ::frc971::control_loops::drivetrain::Output &output) {
+ for (auto falcon : right_falcons_) {
+ falcon->WriteVoltage(output.right_voltage());
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteVoltage(output.left_voltage());
+ }
+}
+
+void CANDrivetrainWriter::Stop() {
+ AOS_LOG(WARNING, "Drivetrain CAN output too old.\n");
+ for (auto falcon : right_falcons_) {
+ falcon->WriteVoltage(0);
+ }
+
+ for (auto falcon : left_falcons_) {
+ falcon->WriteVoltage(0);
+ }
+}
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