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>
diff --git a/y2023/BUILD b/y2023/BUILD
index bdeaea9..f055fe4 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -1,6 +1,7 @@
load("//frc971:downloader.bzl", "robot_downloader")
load("//aos:config.bzl", "aos_config")
load("//tools/build_rules:template.bzl", "jinja2_template")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
robot_downloader(
binaries = [
@@ -185,6 +186,7 @@
name = "config_roborio",
src = "y2023_roborio.json",
flatbuffers = [
+ ":can_configuration_fbs",
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
@@ -250,6 +252,7 @@
],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
+ ":can_configuration_fbs",
":constants",
"//aos:init",
"//aos:math",
@@ -341,3 +344,12 @@
"@com_github_google_glog//:glog",
],
)
+
+flatbuffer_cc_library(
+ name = "can_configuration_fbs",
+ srcs = [
+ ":can_configuration.fbs",
+ ],
+ gen_reflections = 1,
+ visibility = ["//visibility:public"],
+)
diff --git a/y2023/can_configuration.fbs b/y2023/can_configuration.fbs
new file mode 100644
index 0000000..75e2691
--- /dev/null
+++ b/y2023/can_configuration.fbs
@@ -0,0 +1,10 @@
+namespace y2023;
+
+// Message which triggers wpilib_interface to print out the current
+// configuration, and optionally re-apply it.
+table CANConfiguration {
+ // If true, re-apply the configs to see if that fixes the falcon.
+ reapply:bool = false (id: 0);
+}
+
+root_type CANConfiguration;
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());
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index 0ed62c7..ac7845f 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -445,6 +445,12 @@
"frequency": 50
},
{
+ "name": "/roborio",
+ "type": "y2023.CANConfiguration",
+ "source_node": "roborio",
+ "frequency": 2
+ },
+ {
"name": "/roborio/constants",
"type": "y2023.Constants",
"source_node": "roborio",