Add a generic CAN writer class
This is meant to replace the superstructure writer logic in
wpilib_interface but it can work for anything that wants to write to CAN
and doesn't want to use the CANDrivetrainWriter so its called generic.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I1926f71355914406132b2994a973b8a32bd58a0d
diff --git a/frc971/wpilib/generic_can_writer.h b/frc971/wpilib/generic_can_writer.h
new file mode 100644
index 0000000..8bacfbd
--- /dev/null
+++ b/frc971/wpilib/generic_can_writer.h
@@ -0,0 +1,70 @@
+#include <map>
+#include <string_view>
+
+#include "frc971/can_configuration_generated.h"
+#include "frc971/wpilib/falcon.h"
+#include "frc971/wpilib/loop_output_handler.h"
+
+namespace frc971 {
+namespace wpilib {
+
+/// This class uses a callback whenever it writes so that the caller can use any
+/// flatbuffer to write to the falcon.
+template <typename T>
+class GenericCANWriter : public LoopOutputHandler<T> {
+ public:
+ GenericCANWriter(
+ ::aos::EventLoop *event_loop,
+ std::function<
+ void(const T &output,
+ std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ write_callback)
+ : LoopOutputHandler<T>(event_loop, "/superstructure"),
+ write_callback_(write_callback) {
+ event_loop->SetRuntimeRealtimePriority(kGenericCANWriterPriority);
+
+ event_loop->OnRun([this]() { WriteConfigs(); });
+ }
+
+ void HandleCANConfiguration(const CANConfiguration &configuration) {
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->PrintConfigs();
+ }
+
+ if (configuration.reapply()) {
+ WriteConfigs();
+ }
+ }
+
+ void add_falcon(std::string_view name, std::shared_ptr<Falcon> falcon) {
+ falcon_map_.insert({name, std::move(falcon)});
+ }
+
+ static constexpr int kGenericCANWriterPriority = 35;
+
+ private:
+ void WriteConfigs() {
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->WriteConfigs();
+ }
+ }
+
+ void Write(const T &output) override { write_callback_(output, falcon_map_); }
+
+ void Stop() override {
+ AOS_LOG(WARNING, "Generic CAN output too old.\n");
+ for (auto &[_, falcon] : falcon_map_) {
+ falcon->WriteVoltage(0);
+ }
+ }
+
+ // Maps each name to a falcon to let the caller retreive them when writing
+ std::map<std::string_view, std::shared_ptr<Falcon>> falcon_map_;
+
+ std::function<void(const T &output,
+ std::map<std::string, std::shared_ptr<Falcon>> falcon_map)>
+ write_callback_;
+};
+
+} // namespace wpilib
+} // namespace frc971