commit bb4c9ac6f9b41b6ef9412c18a087ec772bb3a116
author Austin Schuh <austin.linux@gmail.com> Tue Feb 28 22:04:20 2023 -0800
committer Austin Schuh <austin.linux@gmail.com> Tue Feb 28 22:09:05 2023 -0800
tree 0babdb033d2774548abb9507ae18ab885ee849e0
parent 0ef334c77ca91b2cdf63b38e7528598cf5f3157c

Add ability to query falcon configuration dynamically

Sometimes, one of the motors appears to not invert correctly.  CTRE is
advising us to double check the config is applying correctly, and try
re-applying it.  This lets us trigger that by hand so we can more easily
detect if it is happening and fix it.

Change-Id: I5236a8202e6c4e5b514d656e055dc597bc861b08
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/wpilib_interface.cc

diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 8e29355..a1a60ba 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -53,6 +53,7 @@
 #include "frc971/wpilib/pdp_fetcher.h"
 #include "frc971/wpilib/sensor_reader.h"
 #include "frc971/wpilib/wpilib_robot_base.h"
+#include "y2023/can_configuration_generated.h"
 #include "y2023/constants.h"
 #include "y2023/control_loops/drivetrain/drivetrain_can_position_generated.h"
 #include "y2023/control_loops/superstructure/superstructure_output_generated.h"
@@ -148,6 +149,17 @@
     signals->push_back(&position_);
   }
 
+  void PrintConfigs() {
+    ctre::phoenixpro::configs::TalonFXConfiguration configuration;
+    ctre::phoenix::StatusCode status =
+        talon_.GetConfigurator().Refresh(configuration);
+    if (!status.IsOK()) {
+      AOS_LOG(ERROR, "Failed to get falcon configuration: %s: %s",
+              status.GetName(), status.GetDescription());
+    }
+    AOS_LOG(INFO, "configuration: %s", configuration.ToString().c_str());
+  }
+
   void WriteConfigs(ctre::phoenixpro::signals::InvertedValue invert) {
     inverted_ = invert;
 
@@ -176,6 +188,8 @@
       AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
               status.GetName(), status.GetDescription());
     }
+
+    PrintConfigs();
   }
 
   void WriteRollerConfigs() {
@@ -202,6 +216,8 @@
       AOS_LOG(ERROR, "Failed to set falcon configuration: %s: %s",
               status.GetName(), status.GetDescription());
     }
+
+    PrintConfigs();
   }
 
   ctre::phoenixpro::hardware::TalonFX *talon() { return &talon_; }
@@ -739,6 +755,13 @@
     event_loop->OnRun([this]() { WriteConfigs(); });
   };
 
+  void HandleCANConfiguration(const CANConfiguration &configuration) {
+    roller_falcon_->PrintConfigs();
+    if (configuration.reapply()) {
+      WriteConfigs();
+    }
+  }
+
   void set_roller_falcon(std::shared_ptr<Falcon> roller_falcon) {
     roller_falcon_ = std::move(roller_falcon);
   }
@@ -810,6 +833,16 @@
     left_inverted_ = invert;
   }
 
+  void HandleCANConfiguration(const CANConfiguration &configuration) {
+    for (auto falcon : {right_front_, right_back_, right_under_, left_front_,
+                        left_back_, left_under_}) {
+      falcon->PrintConfigs();
+    }
+    if (configuration.reapply()) {
+      WriteConfigs();
+    }
+  }
+
  private:
   void WriteConfigs() {
     for (auto falcon :
@@ -990,6 +1023,13 @@
     SuperstructureCANWriter superstructure_can_writer(&can_output_event_loop);
     superstructure_can_writer.set_roller_falcon(roller);
 
+    can_output_event_loop.MakeWatcher(
+        "/roborio", [&drivetrain_writer, &superstructure_can_writer](
+                        const CANConfiguration &configuration) {
+          drivetrain_writer.HandleCANConfiguration(configuration);
+          superstructure_can_writer.HandleCANConfiguration(configuration);
+        });
+
     AddLoop(&can_output_event_loop);
 
     ::aos::ShmEventLoop output_event_loop(&config.message());