Merge "Add ability to query falcon configuration dynamically"
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",