Update swerve drivetrain position messages & logic
This makes it so that the swerve drivetrain positions are sent in two
separate position messages (for FPGA-based and CAN-based positions),
using message definitions from the frc971/ folder.
Also refactors the SwerveModule class/usage a bit.
Removes the follower wheels from the y2024_swerve code.
Change-Id: I36898c3337b5a1437ce0c2a0189fd317929f1986
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index 4e06932..ea6dd7c 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -8,6 +8,13 @@
)
static_flatbuffer(
+ name = "swerve_drivetrain_can_position_fbs",
+ srcs = ["swerve_drivetrain_can_position.fbs"],
+ visibility = ["//visibility:public"],
+ deps = ["//frc971/control_loops:can_talonfx_fbs"],
+)
+
+static_flatbuffer(
name = "swerve_drivetrain_position_fbs",
srcs = ["swerve_drivetrain_position.fbs"],
visibility = ["//visibility:public"],
diff --git a/frc971/control_loops/swerve/swerve_drivetrain_can_position.fbs b/frc971/control_loops/swerve/swerve_drivetrain_can_position.fbs
new file mode 100644
index 0000000..957bce8
--- /dev/null
+++ b/frc971/control_loops/swerve/swerve_drivetrain_can_position.fbs
@@ -0,0 +1,18 @@
+include "frc971/control_loops/can_talonfx.fbs";
+
+namespace frc971.control_loops.swerve;
+
+table SwerveModuleCanPosition {
+ rotation: frc971.control_loops.CANTalonFX (id: 0);
+ translation: frc971.control_loops.CANTalonFX (id: 1);
+}
+
+// CAN Readings from the CAN sensor reader loop for each swerve module
+table CanPosition {
+ front_left: SwerveModuleCanPosition (id: 0);
+ front_right: SwerveModuleCanPosition (id: 1);
+ back_left: SwerveModuleCanPosition (id: 2);
+ back_right: SwerveModuleCanPosition (id: 3);
+}
+
+root_type CanPosition;
diff --git a/frc971/control_loops/swerve/swerve_drivetrain_output.fbs b/frc971/control_loops/swerve/swerve_drivetrain_output.fbs
index 43ba0ed..72517a6 100644
--- a/frc971/control_loops/swerve/swerve_drivetrain_output.fbs
+++ b/frc971/control_loops/swerve/swerve_drivetrain_output.fbs
@@ -1,4 +1,4 @@
-namespace frc971.control_loops.drivetrain.swerve;
+namespace frc971.control_loops.swerve;
table SwerveModuleOutput {
// Current in Amps.
diff --git a/frc971/control_loops/swerve/swerve_drivetrain_position.fbs b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
index bad8af1..a5c921a 100644
--- a/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
+++ b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
@@ -1,17 +1,19 @@
-namespace frc971.control_loops.drivetrain.swerve;
+include "frc971/control_loops/control_loops.fbs";
+
+namespace frc971.control_loops.swerve;
table SwerveModulePosition {
- // Rotation in radians
- rotation_encoder:double (id: 0);
- // Translation in meters
- translation_encoder:double (id: 1);
+ // Position of the mag encoder for the rotation of the module.
+ rotation_position: frc971.AbsolutePosition (id: 0);
}
+// Captures all of the roborio-sourced position information for a
+// swerve drivetrain.
table Position {
- front_left_position:SwerveModulePosition (id: 0);
- front_right_position:SwerveModulePosition (id: 1);
- back_left_position:SwerveModulePosition (id: 2);
- back_right_position:SwerveModulePosition (id: 3);
+ front_left:SwerveModulePosition (id: 0);
+ front_right:SwerveModulePosition (id: 1);
+ back_left:SwerveModulePosition (id: 2);
+ back_right:SwerveModulePosition (id: 3);
}
root_type Position;
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 8eb5651..0b2cc1c 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -12,7 +12,9 @@
":swerve_module",
"//aos/logging",
"//frc971:can_configuration_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
"//frc971/wpilib:loop_output_handler",
"//frc971/wpilib:talonfx",
"//third_party:phoenix6",
@@ -25,6 +27,7 @@
"swerve_module.h",
],
deps = [
+ "//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:talonfx",
],
)
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
index e4b4ee9..ccadb99 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.cc
@@ -6,27 +6,21 @@
int drivetrain_writer_priority,
double max_voltage)
: ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::drivetrain::swerve::Output>(event_loop,
- "/drivetrain"),
+ ::frc971::control_loops::swerve::Output>(event_loop, "/drivetrain"),
max_voltage_(max_voltage) {
event_loop->SetRuntimeRealtimePriority(drivetrain_writer_priority);
event_loop->OnRun([this]() { WriteConfigs(); });
}
-void DrivetrainWriter::set_talonfxs(std::shared_ptr<SwerveModule> front_left,
- std::shared_ptr<SwerveModule> front_right,
- std::shared_ptr<SwerveModule> back_left,
- std::shared_ptr<SwerveModule> back_right) {
- front_left_ = std::move(front_left);
- front_right_ = std::move(front_right);
- back_left_ = std::move(back_left);
- back_right_ = std::move(back_right);
+void DrivetrainWriter::set_talonfxs(SwerveModules modules) {
+ modules = std::move(modules);
}
void DrivetrainWriter::HandleCANConfiguration(
const CANConfiguration &configuration) {
- for (auto module : {front_left_, front_right_, back_left_, back_right_}) {
+ for (auto module : {modules_.front_left, modules_.front_right,
+ modules_.back_left, modules_.back_right}) {
module->rotation->PrintConfigs();
module->translation->PrintConfigs();
}
@@ -36,24 +30,26 @@
}
void DrivetrainWriter::WriteConfigs() {
- for (auto module : {front_left_, front_right_, back_left_, back_right_}) {
+ for (auto module : {modules_.front_left, modules_.front_right,
+ modules_.back_left, modules_.back_right}) {
module->rotation->WriteConfigs();
module->translation->WriteConfigs();
}
}
void DrivetrainWriter::Write(
- const ::frc971::control_loops::drivetrain::swerve::Output &output) {
- front_left_->WriteModule(output.front_left_output(), max_voltage_);
- front_right_->WriteModule(output.front_right_output(), max_voltage_);
- back_left_->WriteModule(output.back_left_output(), max_voltage_);
- back_right_->WriteModule(output.back_right_output(), max_voltage_);
+ const ::frc971::control_loops::swerve::Output &output) {
+ modules_.front_left->WriteModule(output.front_left_output(), max_voltage_);
+ modules_.front_right->WriteModule(output.front_right_output(), max_voltage_);
+ modules_.back_left->WriteModule(output.back_left_output(), max_voltage_);
+ modules_.back_right->WriteModule(output.back_right_output(), max_voltage_);
}
void DrivetrainWriter::Stop() {
AOS_LOG(WARNING, "drivetrain output too old\n");
- for (auto module : {front_left_, front_right_, back_left_, back_right_}) {
+ for (auto module : {modules_.front_left, modules_.front_right,
+ modules_.back_left, modules_.back_right}) {
module->rotation->WriteCurrent(0, 0);
module->translation->WriteCurrent(0, 0);
}
diff --git a/frc971/wpilib/swerve/swerve_drivetrain_writer.h b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
index 16db158..3b79186 100644
--- a/frc971/wpilib/swerve/swerve_drivetrain_writer.h
+++ b/frc971/wpilib/swerve/swerve_drivetrain_writer.h
@@ -13,32 +13,26 @@
// Reads from the swerve output flatbuffer and uses wpilib to set the current
// for each motor.
-class DrivetrainWriter
- : public ::frc971::wpilib::LoopOutputHandler<
- ::frc971::control_loops::drivetrain::swerve::Output> {
+class DrivetrainWriter : public ::frc971::wpilib::LoopOutputHandler<
+ ::frc971::control_loops::swerve::Output> {
public:
DrivetrainWriter(::aos::EventLoop *event_loop, int drivetrain_writer_priority,
double max_voltage);
- void set_talonfxs(std::shared_ptr<SwerveModule> front_left,
- std::shared_ptr<SwerveModule> front_right,
- std::shared_ptr<SwerveModule> back_left,
- std::shared_ptr<SwerveModule> back_right);
+ void set_talonfxs(SwerveModules modules);
void HandleCANConfiguration(const CANConfiguration &configuration);
private:
void WriteConfigs();
- void Write(const ::frc971::control_loops::drivetrain::swerve::Output &output)
- override;
+ void Write(const ::frc971::control_loops::swerve::Output &output) override;
void Stop() override;
double SafeSpeed(double voltage);
- std::shared_ptr<SwerveModule> front_left_, front_right_, back_left_,
- back_right_;
+ SwerveModules modules_;
double max_voltage_;
};
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index 729ebab..c01df24 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -1,11 +1,17 @@
#ifndef FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
#define FRC971_WPILIB_SWERVE_SWERVE_MODULE_H_
+#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_static.h"
#include "frc971/control_loops/swerve/swerve_drivetrain_output_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_position_static.h"
+#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/talonfx.h"
namespace frc971::wpilib::swerve {
+// Contains the objects for interacting with the hardware for a given swerve
+// module, assuming that the module uses two TalonFX-based motor controllers and
+// has a CTRE mag encoder on the rotation of the module.
struct SwerveModule {
SwerveModule(TalonFXParams rotation_params, TalonFXParams translation_params,
std::string canbus,
@@ -18,9 +24,10 @@
signals, stator_current_limit,
supply_current_limit)) {}
+ // Writes the requested torque currents from the module_output to the motors,
+ // setting the maximum voltage of the motor outputs to the requested value.
void WriteModule(
- const frc971::control_loops::drivetrain::swerve::SwerveModuleOutput
- *module_output,
+ const frc971::control_loops::swerve::SwerveModuleOutput *module_output,
double max_voltage) {
double rotation_current = 0.0;
double translation_current = 0.0;
@@ -34,8 +41,58 @@
translation->WriteCurrent(translation_current, max_voltage);
}
+ // Used during initialization to set the WPILib objects used by the mag
+ // encoder on the rotation joint.
+ void set_rotation_encoder(std::unique_ptr<frc::Encoder> encoder,
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
+ rotation_encoder.set_encoder(std::move(encoder));
+ rotation_encoder.set_absolute_pwm(std::move(absolute_pwm));
+ }
+
+ // Populates the Position message with the mag encoder values.
+ void PopulatePosition(
+ frc971::control_loops::swerve::SwerveModulePositionStatic *fbs) {
+ auto rotation_position = fbs->add_rotation_position();
+ rotation_position->set_encoder(rotation_encoder.ReadRelativeEncoder());
+ rotation_position->set_absolute_encoder(
+ rotation_encoder.ReadAbsoluteEncoder());
+ }
+
+ // Populates a CAN-position message with the CAN-based devices (currently,
+ // just the motors themselves).
+ void PopulateCanPosition(
+ frc971::control_loops::swerve::SwerveModuleCanPositionStatic
+ *can_position) {
+ // TODO(james): Source these numbers from the constants.json.
+ rotation->SerializePosition(can_position->add_rotation(), 1.0);
+ translation->SerializePosition(can_position->add_translation(), 1.0);
+ }
+
std::shared_ptr<TalonFX> rotation;
std::shared_ptr<TalonFX> translation;
+ frc971::wpilib::AbsoluteEncoder rotation_encoder;
+};
+
+// Represents all the modules in a swerve drivetrain.
+struct SwerveModules {
+ void PopulateFalconsVector(std::vector<std::shared_ptr<TalonFX>> *falcons) {
+ CHECK_NOTNULL(falcons)->push_back(front_left->rotation);
+ falcons->push_back(front_left->translation);
+
+ falcons->push_back(front_right->rotation);
+ falcons->push_back(front_right->translation);
+
+ falcons->push_back(back_left->rotation);
+ falcons->push_back(back_left->translation);
+
+ falcons->push_back(back_right->rotation);
+ falcons->push_back(back_right->translation);
+ }
+
+ std::shared_ptr<SwerveModule> front_left;
+ std::shared_ptr<SwerveModule> front_right;
+ std::shared_ptr<SwerveModule> back_left;
+ std::shared_ptr<SwerveModule> back_right;
};
} // namespace frc971::wpilib::swerve
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index ee9c3cb..b24c469 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -1,5 +1,4 @@
load("//aos:config.bzl", "aos_config")
-load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
load("//aos/util:config_validator_macro.bzl", "config_validator_test")
load("//frc971:downloader.bzl", "robot_downloader")
@@ -106,18 +105,6 @@
],
)
-static_flatbuffer(
- name = "drivetrain_position_fbs",
- srcs = ["drivetrain_position.fbs"],
- deps = ["//frc971/control_loops:control_loops_fbs"],
-)
-
-static_flatbuffer(
- name = "drivetrain_can_position_fbs",
- srcs = ["drivetrain_can_position.fbs"],
- deps = ["//frc971/control_loops:can_talonfx_fbs"],
-)
-
cc_binary(
name = "swerve_publisher",
srcs = ["swerve_publisher_main.cc"],
@@ -162,11 +149,10 @@
target_compatible_with = ["//tools/platforms/hardware:roborio"],
deps = [
":constants",
- ":drivetrain_can_position_fbs",
- ":drivetrain_position_fbs",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops:control_loops_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
"//frc971/wpilib:can_sensor_reader",
"//frc971/wpilib:sensor_reader",
@@ -200,8 +186,6 @@
name = "config_roborio",
src = "y2024_swerve_roborio.json",
flatbuffers = [
- ":drivetrain_position_fbs",
- ":drivetrain_can_position_fbs",
"//frc971:can_configuration_fbs",
"//aos/network:remote_message_fbs",
"//aos/network:message_bridge_client_fbs",
@@ -209,6 +193,7 @@
"//aos/network:timestamp_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_output_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
+ "//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_can_position_fbs",
"//frc971/can_logger:can_logging_fbs",
],
diff --git a/y2024_swerve/drivetrain_can_position.fbs b/y2024_swerve/drivetrain_can_position.fbs
deleted file mode 100644
index f0d41a8..0000000
--- a/y2024_swerve/drivetrain_can_position.fbs
+++ /dev/null
@@ -1,17 +0,0 @@
-include "frc971/control_loops/can_talonfx.fbs";
-namespace y2024_swerve;
-
-table SwerveModuleCANPosition {
- rotation: frc971.control_loops.CANTalonFX (id: 0);
- translation: frc971.control_loops.CANTalonFX (id: 1);
-}
-
-// CAN Readings from the CAN sensor reader loop for each swerve module
-table AbsoluteCANPosition {
- front_left: SwerveModuleCANPosition (id: 0);
- front_right: SwerveModuleCANPosition (id: 1);
- back_left: SwerveModuleCANPosition (id: 2);
- back_right: SwerveModuleCANPosition (id: 3);
-}
-
-root_type AbsoluteCANPosition;
diff --git a/y2024_swerve/drivetrain_position.fbs b/y2024_swerve/drivetrain_position.fbs
deleted file mode 100644
index 902cfee..0000000
--- a/y2024_swerve/drivetrain_position.fbs
+++ /dev/null
@@ -1,16 +0,0 @@
-include "frc971/control_loops/control_loops.fbs";
-namespace y2024_swerve;
-
-table AbsoluteDrivetrainPosition {
- // Position of the follower wheels from the encoders
- follower_wheel_one_position:double (id: 0);
- follower_wheel_two_position:double (id: 1);
-
- // Position from the mag encoder on each module.
- front_left_position: frc971.AbsolutePosition (id: 2);
- front_right_position: frc971.AbsolutePosition (id: 3);
- back_left_position: frc971.AbsolutePosition (id: 4);
- back_right_position: frc971.AbsolutePosition (id: 5);
-}
-
-root_type AbsoluteDrivetrainPosition;
diff --git a/y2024_swerve/swerve_publisher_lib.cc b/y2024_swerve/swerve_publisher_lib.cc
index 771d770..2eb9cce 100644
--- a/y2024_swerve/swerve_publisher_lib.cc
+++ b/y2024_swerve/swerve_publisher_lib.cc
@@ -5,16 +5,18 @@
const std::string &filename,
double duration)
: drivetrain_output_sender_(
- event_loop->MakeSender<drivetrain::swerve::Output>("/drivetrain")) {
+ event_loop->MakeSender<frc971::control_loops::swerve::Output>(
+ "/drivetrain")) {
event_loop
->AddTimer([this, filename]() {
auto output_builder = drivetrain_output_sender_.MakeBuilder();
auto drivetrain_output =
- aos::JsonFileToFlatbuffer<drivetrain::swerve::Output>(filename);
+ aos::JsonFileToFlatbuffer<frc971::control_loops::swerve::Output>(
+ filename);
auto copied_flatbuffer =
- aos::CopyFlatBuffer<drivetrain::swerve::Output>(
+ aos::CopyFlatBuffer<frc971::control_loops::swerve::Output>(
drivetrain_output, output_builder.fbb());
CHECK(drivetrain_output.Verify());
@@ -26,16 +28,18 @@
event_loop
->AddTimer([this, exit_handle]() {
auto builder = drivetrain_output_sender_.MakeBuilder();
- drivetrain::swerve::SwerveModuleOutput::Builder swerve_module_builder =
- builder.MakeBuilder<drivetrain::swerve::SwerveModuleOutput>();
+ 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();
- drivetrain::swerve::Output::Builder drivetrain_output_builder =
- builder.MakeBuilder<drivetrain::swerve::Output>();
+ 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);
diff --git a/y2024_swerve/swerve_publisher_lib.h b/y2024_swerve/swerve_publisher_lib.h
index 1a07865..e577455 100644
--- a/y2024_swerve/swerve_publisher_lib.h
+++ b/y2024_swerve/swerve_publisher_lib.h
@@ -12,16 +12,13 @@
namespace y2024_swerve {
-namespace drivetrain = frc971::control_loops::drivetrain;
-
class SwervePublisher {
public:
SwervePublisher(aos::EventLoop *event_loop, aos::ExitHandle *exit_handle,
const std::string &filename, double duration);
private:
- aos::Sender<frc971::control_loops::drivetrain::swerve::Output>
- drivetrain_output_sender_;
+ aos::Sender<frc971::control_loops::swerve::Output> drivetrain_output_sender_;
};
} // namespace y2024_swerve
diff --git a/y2024_swerve/swerve_publisher_lib_test.cc b/y2024_swerve/swerve_publisher_lib_test.cc
index 95892cc..096dc9e 100644
--- a/y2024_swerve/swerve_publisher_lib_test.cc
+++ b/y2024_swerve/swerve_publisher_lib_test.cc
@@ -16,8 +16,7 @@
event_loop_factory_.MakeEventLoop("swerve_publisher", roborio_)),
exit_handle_(event_loop_factory_.MakeExitHandle()),
drivetrain_swerve_output_fetcher_(
- event_loop_->MakeFetcher<
- frc971::control_loops::drivetrain::swerve::Output>(
+ event_loop_->MakeFetcher<frc971::control_loops::swerve::Output>(
"/drivetrain")),
swerve_publisher_(event_loop_.get(), exit_handle_.get(),
"y2024_swerve/swerve_drivetrain_output.json", 100) {}
@@ -39,7 +38,7 @@
std::unique_ptr<aos::EventLoop> event_loop_;
std::unique_ptr<aos::ExitHandle> exit_handle_;
- aos::Fetcher<frc971::control_loops::drivetrain::swerve::Output>
+ aos::Fetcher<frc971::control_loops::swerve::Output>
drivetrain_swerve_output_fetcher_;
y2024_swerve::SwervePublisher swerve_publisher_;
diff --git a/y2024_swerve/wpilib_interface.cc b/y2024_swerve/wpilib_interface.cc
index 65741a4..853164f 100644
--- a/y2024_swerve/wpilib_interface.cc
+++ b/y2024_swerve/wpilib_interface.cc
@@ -4,14 +4,14 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_can_position_static.h"
+#include "frc971/control_loops/swerve/swerve_drivetrain_position_static.h"
#include "frc971/wpilib/can_sensor_reader.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/swerve/swerve_drivetrain_writer.h"
#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024_swerve/constants.h"
-#include "y2024_swerve/drivetrain_can_position_static.h"
-#include "y2024_swerve/drivetrain_position_generated.h"
DEFINE_bool(ctre_diag_server, false,
"If true, enable the diagnostics server for interacting with "
@@ -33,24 +33,6 @@
return optional.value();
}
-flatbuffers::Offset<frc971::AbsolutePosition> module_offset(
- frc971::AbsolutePosition::Builder builder,
- frc971::wpilib::AbsoluteEncoder *module) {
- builder.add_encoder(module->ReadRelativeEncoder());
- builder.add_absolute_encoder(module->ReadAbsoluteEncoder());
-
- return builder.Finish();
-}
-
-void populate_can_module(SwerveModuleCANPositionStatic *can_position,
- std::shared_ptr<SwerveModule> module) {
- auto rotation = can_position->add_rotation();
- module->rotation->SerializePosition(rotation, 1.0);
-
- auto translation = can_position->add_translation();
- module->translation->SerializePosition(translation, 1.0);
-}
-
constexpr double kMaxFastEncoderPulsesPerSecond = std::max({
constants::Values::kMaxDrivetrainEncoderPulsesPerSecond(),
});
@@ -61,105 +43,68 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(aos::ShmEventLoop *event_loop,
- std::shared_ptr<const constants::Values> values)
+ std::shared_ptr<const constants::Values> values,
+ frc971::wpilib::swerve::SwerveModules modules)
: ::frc971::wpilib::SensorReader(event_loop),
values_(values),
drivetrain_position_sender_(
- event_loop->MakeSender<AbsoluteDrivetrainPosition>("/drivetrain")) {
+ event_loop
+ ->MakeSender<frc971::control_loops::swerve::PositionStatic>(
+ "/drivetrain")),
+ modules_(modules) {
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
event_loop->SetRuntimeAffinity(aos::MakeCpusetFromCpus({0}));
}
void RunIteration() override {
{
- auto builder = drivetrain_position_sender_.MakeBuilder();
+ auto builder = drivetrain_position_sender_.MakeStaticBuilder();
- auto front_left_offset =
- module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
- &front_left_encoder_);
- auto front_right_offset =
- module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
- &front_right_encoder_);
- auto back_left_offset = module_offset(
- builder.MakeBuilder<frc971::AbsolutePosition>(), &back_left_encoder_);
- auto back_right_offset =
- module_offset(builder.MakeBuilder<frc971::AbsolutePosition>(),
- &back_right_encoder_);
+ modules_.front_left->PopulatePosition(builder->add_front_left());
+ modules_.front_right->PopulatePosition(builder->add_front_right());
+ modules_.back_left->PopulatePosition(builder->add_back_left());
+ modules_.back_right->PopulatePosition(builder->add_back_right());
- AbsoluteDrivetrainPosition::Builder drivetrain_position_builder =
- builder.MakeBuilder<AbsoluteDrivetrainPosition>();
-
- drivetrain_position_builder.add_follower_wheel_one_position(
- follower_wheel_one_encoder_->GetRaw());
- drivetrain_position_builder.add_follower_wheel_two_position(
- follower_wheel_two_encoder_->GetRaw());
-
- drivetrain_position_builder.add_front_left_position(front_left_offset);
- drivetrain_position_builder.add_front_right_position(front_right_offset);
- drivetrain_position_builder.add_back_left_position(back_left_offset);
- drivetrain_position_builder.add_back_right_position(back_right_offset);
-
- builder.CheckOk(builder.Send(drivetrain_position_builder.Finish()));
+ builder.CheckOk(builder.Send());
}
}
- void set_follower_wheel_one_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ void set_front_left_encoder(std::unique_ptr<frc::Encoder> encoder,
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
fast_encoder_filter_.Add(encoder.get());
- follower_wheel_one_encoder_ = std::move(encoder);
- follower_wheel_one_encoder_->SetMaxPeriod(0.005);
- }
- void set_follower_wheel_two_encoder(std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- follower_wheel_two_encoder_ = std::move(encoder);
- follower_wheel_two_encoder_->SetMaxPeriod(0.005);
+ modules_.front_left->set_rotation_encoder(std::move(encoder),
+ std::move(absolute_pwm));
}
- void set_front_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- front_left_encoder_.set_encoder(std::move(encoder));
- }
- void set_front_left_absolute_pwm(
+ void set_front_right_encoder(
+ std::unique_ptr<frc::Encoder> encoder,
std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- front_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ fast_encoder_filter_.Add(encoder.get());
+ modules_.front_right->set_rotation_encoder(std::move(encoder),
+ std::move(absolute_pwm));
}
- void set_front_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ void set_back_left_encoder(std::unique_ptr<frc::Encoder> encoder,
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
fast_encoder_filter_.Add(encoder.get());
- front_right_encoder_.set_encoder(std::move(encoder));
- }
- void set_front_right_absolute_pwm(
- std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- front_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ modules_.back_left->set_rotation_encoder(std::move(encoder),
+ std::move(absolute_pwm));
}
- void set_back_left_encoder(std::unique_ptr<frc::Encoder> encoder) {
+ void set_back_right_encoder(std::unique_ptr<frc::Encoder> encoder,
+ std::unique_ptr<frc::DigitalInput> absolute_pwm) {
fast_encoder_filter_.Add(encoder.get());
- back_left_encoder_.set_encoder(std::move(encoder));
- }
- void set_back_left_absolute_pwm(
- std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- back_left_encoder_.set_absolute_pwm(std::move(absolute_pwm));
- }
-
- void set_back_right_encoder(std::unique_ptr<frc::Encoder> encoder) {
- fast_encoder_filter_.Add(encoder.get());
- back_right_encoder_.set_encoder(std::move(encoder));
- }
- void set_back_right_absolute_pwm(
- std::unique_ptr<frc::DigitalInput> absolute_pwm) {
- back_right_encoder_.set_absolute_pwm(std::move(absolute_pwm));
+ modules_.back_right->set_rotation_encoder(std::move(encoder),
+ std::move(absolute_pwm));
}
private:
std::shared_ptr<const constants::Values> values_;
- aos::Sender<AbsoluteDrivetrainPosition> drivetrain_position_sender_;
+ aos::Sender<frc971::control_loops::swerve::PositionStatic>
+ drivetrain_position_sender_;
- std::unique_ptr<frc::Encoder> follower_wheel_one_encoder_,
- follower_wheel_two_encoder_;
-
- frc971::wpilib::AbsoluteEncoder front_left_encoder_, front_right_encoder_,
- back_left_encoder_, back_right_encoder_;
+ frc971::wpilib::swerve::SwerveModules modules_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -179,70 +124,62 @@
std::vector<std::shared_ptr<TalonFX>> falcons;
// TODO(max): Change the CanBus names with TalonFX software.
- std::shared_ptr<SwerveModule> front_left = std::make_shared<SwerveModule>(
- frc971::wpilib::TalonFXParams{6, false},
- frc971::wpilib::TalonFXParams{5, false}, "Drivetrain Bus",
- &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<SwerveModule> front_right = std::make_shared<SwerveModule>(
- frc971::wpilib::TalonFXParams{3, false},
- frc971::wpilib::TalonFXParams{4, false}, "Drivetrain Bus",
- &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<SwerveModule> back_left = std::make_shared<SwerveModule>(
- frc971::wpilib::TalonFXParams{8, false},
- frc971::wpilib::TalonFXParams{7, false}, "Drivetrain Bus",
- &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
- std::shared_ptr<SwerveModule> back_right = std::make_shared<SwerveModule>(
- frc971::wpilib::TalonFXParams{2, false},
- frc971::wpilib::TalonFXParams{1, false}, "Drivetrain Bus",
- &signals_registry, constants::Values::kDrivetrainStatorCurrentLimit(),
- constants::Values::kDrivetrainSupplyCurrentLimit());
+ frc971::wpilib::swerve::SwerveModules modules{
+ .front_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::TalonFXParams{6, false},
+ frc971::wpilib::TalonFXParams{5, false}, "Drivetrain Bus",
+ &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit()),
+ .front_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::TalonFXParams{3, false},
+ frc971::wpilib::TalonFXParams{4, false}, "Drivetrain Bus",
+ &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit()),
+ .back_left = std::make_shared<SwerveModule>(
+ frc971::wpilib::TalonFXParams{8, false},
+ frc971::wpilib::TalonFXParams{7, false}, "Drivetrain Bus",
+ &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit()),
+ .back_right = std::make_shared<SwerveModule>(
+ frc971::wpilib::TalonFXParams{2, false},
+ frc971::wpilib::TalonFXParams{1, false}, "Drivetrain Bus",
+ &signals_registry,
+ constants::Values::kDrivetrainStatorCurrentLimit(),
+ constants::Values::kDrivetrainSupplyCurrentLimit())};
// Thread 1
aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
can_sensor_reader_event_loop.set_name("CANSensorReader");
- falcons.push_back(front_left->rotation);
- falcons.push_back(front_left->translation);
+ modules.PopulateFalconsVector(&falcons);
- falcons.push_back(front_right->rotation);
- falcons.push_back(front_right->translation);
-
- falcons.push_back(back_left->rotation);
- falcons.push_back(back_left->translation);
-
- falcons.push_back(back_right->rotation);
- falcons.push_back(back_right->translation);
-
- aos::Sender<AbsoluteCANPositionStatic> can_position_sender =
- can_sensor_reader_event_loop.MakeSender<AbsoluteCANPositionStatic>(
- "/drivetrain");
+ aos::Sender<frc971::control_loops::swerve::CanPositionStatic>
+ can_position_sender =
+ can_sensor_reader_event_loop
+ .MakeSender<frc971::control_loops::swerve::CanPositionStatic>(
+ "/drivetrain");
CANSensorReader can_sensor_reader(
&can_sensor_reader_event_loop, std::move(signals_registry), falcons,
- [this, falcons, front_left, front_right, back_left, back_right,
+ [this, falcons, modules,
&can_position_sender](ctre::phoenix::StatusCode status) {
// TODO(max): use status properly in the flatbuffer.
(void)status;
- aos::Sender<AbsoluteCANPositionStatic>::StaticBuilder builder =
- can_position_sender.MakeStaticBuilder();
+ aos::Sender<frc971::control_loops::swerve::CanPositionStatic>::
+ StaticBuilder builder = can_position_sender.MakeStaticBuilder();
for (auto falcon : falcons) {
falcon->RefreshNontimesyncedSignals();
}
- auto front_left_flatbuffer = builder->add_front_left();
- auto front_right_flatbuffer = builder->add_front_right();
- auto back_left_flatbuffer = builder->add_back_left();
- auto back_right_flatbuffer = builder->add_back_right();
-
- populate_can_module(front_left_flatbuffer, front_left);
- populate_can_module(front_right_flatbuffer, front_right);
- populate_can_module(back_left_flatbuffer, back_left);
- populate_can_module(back_right_flatbuffer, back_right);
+ modules.front_left->PopulateCanPosition(builder->add_front_left());
+ modules.front_right->PopulateCanPosition(builder->add_front_right());
+ modules.back_left->PopulateCanPosition(builder->add_back_left());
+ modules.back_right->PopulateCanPosition(builder->add_back_right());
builder.CheckOk(builder.Send());
});
@@ -268,34 +205,26 @@
&drivetrain_writer_event_loop,
constants::Values::kDrivetrainWriterPriority, 12);
- drivetrain_writer.set_talonfxs(front_left, front_right, back_left,
- back_right);
+ drivetrain_writer.set_talonfxs(modules);
AddLoop(&drivetrain_writer_event_loop);
// Thread 3
aos::ShmEventLoop sensor_reader_event_loop(&config.message());
sensor_reader_event_loop.set_name("SensorReader");
- SensorReader sensor_reader(&sensor_reader_event_loop, values);
+ SensorReader sensor_reader(&sensor_reader_event_loop, values, modules);
- sensor_reader.set_follower_wheel_one_encoder(make_encoder(4));
- sensor_reader.set_follower_wheel_two_encoder(make_encoder(5));
+ sensor_reader.set_front_left_encoder(
+ make_encoder(1), std::make_unique<frc::DigitalInput>(1));
- sensor_reader.set_front_left_encoder(make_encoder(1));
- sensor_reader.set_front_left_absolute_pwm(
- std::make_unique<frc::DigitalInput>(1));
+ sensor_reader.set_front_right_encoder(
+ make_encoder(0), std::make_unique<frc::DigitalInput>(0));
- sensor_reader.set_front_right_encoder(make_encoder(0));
- sensor_reader.set_front_right_absolute_pwm(
- std::make_unique<frc::DigitalInput>(0));
+ sensor_reader.set_back_left_encoder(make_encoder(2),
+ std::make_unique<frc::DigitalInput>(2));
- sensor_reader.set_back_left_encoder(make_encoder(2));
- sensor_reader.set_back_left_absolute_pwm(
- std::make_unique<frc::DigitalInput>(2));
-
- sensor_reader.set_back_right_encoder(make_encoder(3));
- sensor_reader.set_back_right_absolute_pwm(
- std::make_unique<frc::DigitalInput>(3));
+ sensor_reader.set_back_right_encoder(
+ make_encoder(3), std::make_unique<frc::DigitalInput>(3));
AddLoop(&sensor_reader_event_loop);
diff --git a/y2024_swerve/y2024_swerve_roborio.json b/y2024_swerve/y2024_swerve_roborio.json
index 1b06f9f..bd8ddff 100644
--- a/y2024_swerve/y2024_swerve_roborio.json
+++ b/y2024_swerve/y2024_swerve_roborio.json
@@ -103,7 +103,7 @@
},
{
"name": "/drivetrain",
- "type": "y2024_swerve.AbsoluteDrivetrainPosition",
+ "type": "frc971.control_loops.swerve.Position",
"source_node": "roborio",
"frequency": 250,
"num_senders": 1,
@@ -111,7 +111,7 @@
},
{
"name": "/drivetrain",
- "type": "y2024_swerve.AbsoluteCANPosition",
+ "type": "frc971.control_loops.swerve.CanPosition",
"source_node": "roborio",
"frequency": 250,
"num_senders": 1,
@@ -148,15 +148,7 @@
},
{
"name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.swerve.Position",
- "source_node": "roborio",
- "frequency": 400,
- "max_size": 112,
- "num_senders": 2
- },
- {
- "name": "/drivetrain",
- "type": "frc971.control_loops.drivetrain.swerve.Output",
+ "type": "frc971.control_loops.swerve.Output",
"source_node": "roborio",
"frequency": 400,
"max_size": 200,