Update swerve_publisher to use Goal message
Rather than attempting to send raw outputs, send the goal instead. This
also updates the swerve_publisher to handle Ctrl-C's more cleanly.
Change-Id: Ibdfe94d48295c184083c90b8672986433737bfa7
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index 133f11d..79bd079 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -16,7 +16,7 @@
],
data = [
":aos_config",
- ":swerve_publisher_output_json",
+ ":swerve_publisher_goal_json",
"//y2024_swerve/constants:constants.json",
"@ctre_phoenix6_api_cpp_athena//:shared_libraries",
"@ctre_phoenix6_tools_athena//:shared_libraries",
@@ -85,9 +85,9 @@
)
filegroup(
- name = "swerve_publisher_output_json",
+ name = "swerve_publisher_goal_json",
srcs = [
- "swerve_drivetrain_output.json",
+ "swerve_drivetrain_goal.json",
],
visibility = ["//y2024_swerve:__subpackages__"],
)
@@ -127,7 +127,7 @@
deps = [
"//aos:init",
"//aos/events:event_loop",
- "//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_goal_fbs",
"@com_google_absl//absl/log",
"@com_google_absl//absl/log:check",
],
@@ -140,7 +140,7 @@
],
data = [
":aos_config",
- ":swerve_publisher_output_json",
+ ":swerve_publisher_goal_json",
],
deps = [
":swerve_publisher_lib",
diff --git a/y2024_swerve/swerve_drivetrain_goal.json b/y2024_swerve/swerve_drivetrain_goal.json
new file mode 100644
index 0000000..5d1c372
--- /dev/null
+++ b/y2024_swerve/swerve_drivetrain_goal.json
@@ -0,0 +1,22 @@
+{
+ "front_left_goal": {
+ "rotation_angle": 1.5,
+ "translation_control_type_goal": "CURRENT",
+ "translation_current": 5.0
+ },
+ "front_right_goal": {
+ "rotation_angle": -1.5,
+ "translation_control_type_goal": "CURRENT",
+ "translation_current": 5.0
+ },
+ "back_left_goal": {
+ "rotation_angle": -1.5,
+ "translation_control_type_goal": "CURRENT",
+ "translation_current": -5.0
+ },
+ "back_right_goal": {
+ "rotation_angle": 1.5,
+ "translation_control_type_goal": "CURRENT",
+ "translation_current": -5.0
+ }
+}
diff --git a/y2024_swerve/swerve_drivetrain_output.json b/y2024_swerve/swerve_drivetrain_output.json
deleted file mode 100644
index bc152e2..0000000
--- a/y2024_swerve/swerve_drivetrain_output.json
+++ /dev/null
@@ -1,18 +0,0 @@
-{
- "front_left_output": {
- "rotation_current": 0.0,
- "translation_current": 0.0
- },
- "front_right_output": {
- "rotation_current": 0.0,
- "translation_current": 0.0
- },
- "back_left_output": {
- "rotation_current": 0.0,
- "translation_current": 0.0
- },
- "back_right_output": {
- "rotation_current": 0.0,
- "translation_current": 0.0
- }
-}
diff --git a/y2024_swerve/swerve_publisher_lib.cc b/y2024_swerve/swerve_publisher_lib.cc
index 2eb9cce..be38e31 100644
--- a/y2024_swerve/swerve_publisher_lib.cc
+++ b/y2024_swerve/swerve_publisher_lib.cc
@@ -4,52 +4,38 @@
aos::ExitHandle *exit_handle,
const std::string &filename,
double duration)
- : drivetrain_output_sender_(
- event_loop->MakeSender<frc971::control_loops::swerve::Output>(
+ : drivetrain_goal_sender_(
+ event_loop->MakeSender<frc971::control_loops::swerve::GoalStatic>(
"/drivetrain")) {
event_loop
->AddTimer([this, filename]() {
- auto output_builder = drivetrain_output_sender_.MakeBuilder();
+ auto goal_builder = drivetrain_goal_sender_.MakeStaticBuilder();
- auto drivetrain_output =
- aos::JsonFileToFlatbuffer<frc971::control_loops::swerve::Output>(
+ auto drivetrain_goal =
+ aos::JsonFileToFlatbuffer<frc971::control_loops::swerve::Goal>(
filename);
+ CHECK(drivetrain_goal.Verify());
+ CHECK(goal_builder->FromFlatbuffer(&drivetrain_goal.message()));
- auto copied_flatbuffer =
- aos::CopyFlatBuffer<frc971::control_loops::swerve::Output>(
- drivetrain_output, output_builder.fbb());
- CHECK(drivetrain_output.Verify());
-
- output_builder.CheckOk(output_builder.Send(copied_flatbuffer));
+ goal_builder.CheckOk(goal_builder.Send());
})
->Schedule(event_loop->monotonic_now(),
std::chrono::duration_cast<aos::monotonic_clock::duration>(
std::chrono::milliseconds(5)));
- event_loop
- ->AddTimer([this, exit_handle]() {
- auto builder = drivetrain_output_sender_.MakeBuilder();
- frc971::control_loops::swerve::SwerveModuleOutput::Builder
- swerve_module_builder = builder.MakeBuilder<
- frc971::control_loops::swerve::SwerveModuleOutput>();
-
- swerve_module_builder.add_rotation_current(0.0);
- swerve_module_builder.add_translation_current(0.0);
-
- auto swerve_module_offset = swerve_module_builder.Finish();
-
- frc971::control_loops::swerve::Output::Builder
- drivetrain_output_builder =
- builder.MakeBuilder<frc971::control_loops::swerve::Output>();
-
- drivetrain_output_builder.add_front_left_output(swerve_module_offset);
- drivetrain_output_builder.add_front_right_output(swerve_module_offset);
- drivetrain_output_builder.add_back_left_output(swerve_module_offset);
- drivetrain_output_builder.add_back_right_output(swerve_module_offset);
-
- builder.CheckOk(builder.Send(drivetrain_output_builder.Finish()));
-
- exit_handle->Exit();
- })
+ event_loop->AddTimer([exit_handle]() { exit_handle->Exit(); })
->Schedule(event_loop->monotonic_now() +
std::chrono::milliseconds((int)duration));
}
+y2024_swerve::SwervePublisher::~SwervePublisher() {
+ auto builder = drivetrain_goal_sender_.MakeStaticBuilder();
+
+ for (auto module_goal :
+ {builder->add_front_left_goal(), builder->add_front_right_goal(),
+ builder->add_back_left_goal(), builder->add_back_right_goal()}) {
+ module_goal->set_rotation_angle(0.0);
+ module_goal->set_translation_control_type_goal(
+ frc971::control_loops::swerve::TranslationControlTypeGoal::CURRENT);
+ module_goal->set_translation_current(0.0);
+ }
+ builder.CheckOk(builder.Send());
+}
diff --git a/y2024_swerve/swerve_publisher_lib.h b/y2024_swerve/swerve_publisher_lib.h
index e490738..6cae518 100644
--- a/y2024_swerve/swerve_publisher_lib.h
+++ b/y2024_swerve/swerve_publisher_lib.h
@@ -9,7 +9,7 @@
#include "aos/flatbuffer_merge.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
-#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_goal_static.h"
namespace y2024_swerve {
@@ -18,8 +18,11 @@
SwervePublisher(aos::EventLoop *event_loop, aos::ExitHandle *exit_handle,
const std::string &filename, double duration);
+ ~SwervePublisher();
+
private:
- aos::Sender<frc971::control_loops::swerve::Output> drivetrain_output_sender_;
+ aos::Sender<frc971::control_loops::swerve::GoalStatic>
+ drivetrain_goal_sender_;
};
} // namespace y2024_swerve
diff --git a/y2024_swerve/swerve_publisher_lib_test.cc b/y2024_swerve/swerve_publisher_lib_test.cc
index 096dc9e..44125cd 100644
--- a/y2024_swerve/swerve_publisher_lib_test.cc
+++ b/y2024_swerve/swerve_publisher_lib_test.cc
@@ -15,19 +15,19 @@
event_loop_(
event_loop_factory_.MakeEventLoop("swerve_publisher", roborio_)),
exit_handle_(event_loop_factory_.MakeExitHandle()),
- drivetrain_swerve_output_fetcher_(
- event_loop_->MakeFetcher<frc971::control_loops::swerve::Output>(
+ drivetrain_swerve_goal_fetcher_(
+ event_loop_->MakeFetcher<frc971::control_loops::swerve::Goal>(
"/drivetrain")),
swerve_publisher_(event_loop_.get(), exit_handle_.get(),
- "y2024_swerve/swerve_drivetrain_output.json", 100) {}
+ "y2024_swerve/swerve_drivetrain_goal.json", 100) {}
void SendOutput() { event_loop_factory_.Run(); }
void CheckOutput() {
- drivetrain_swerve_output_fetcher_.Fetch();
+ drivetrain_swerve_goal_fetcher_.Fetch();
- ASSERT_TRUE(drivetrain_swerve_output_fetcher_.get() != nullptr)
- << ": No drivetrain output";
+ ASSERT_TRUE(drivetrain_swerve_goal_fetcher_.get() != nullptr)
+ << ": No drivetrain goal.";
}
private:
@@ -38,8 +38,8 @@
std::unique_ptr<aos::EventLoop> event_loop_;
std::unique_ptr<aos::ExitHandle> exit_handle_;
- aos::Fetcher<frc971::control_loops::swerve::Output>
- drivetrain_swerve_output_fetcher_;
+ aos::Fetcher<frc971::control_loops::swerve::Goal>
+ drivetrain_swerve_goal_fetcher_;
y2024_swerve::SwervePublisher swerve_publisher_;
};
diff --git a/y2024_swerve/swerve_publisher_main.cc b/y2024_swerve/swerve_publisher_main.cc
index da7a99f..8789fb9 100644
--- a/y2024_swerve/swerve_publisher_main.cc
+++ b/y2024_swerve/swerve_publisher_main.cc
@@ -4,7 +4,7 @@
#include "y2024_swerve/swerve_publisher_lib.h"
ABSL_FLAG(double, duration, 100.0, "Length of time in Ms to apply current for");
-ABSL_FLAG(std::string, drivetrain_position, "swerve_drivetrain_output.json",
+ABSL_FLAG(std::string, drivetrain_goal, "swerve_drivetrain_goal.json",
"The path to the json drivetrain position to apply");
ABSL_FLAG(std::string, config, "aos_config.json",
"The path to aos_config.json");
@@ -19,9 +19,9 @@
std::unique_ptr<aos::ExitHandle> exit_handle = event_loop.MakeExitHandle();
- y2024_swerve::SwervePublisher publisher(
- &event_loop, exit_handle.get(), absl::GetFlag(FLAGS_drivetrain_position),
- absl::GetFlag(FLAGS_duration));
+ y2024_swerve::SwervePublisher publisher(&event_loop, exit_handle.get(),
+ absl::GetFlag(FLAGS_drivetrain_goal),
+ absl::GetFlag(FLAGS_duration));
event_loop.Run();
}