Scale swerve mag encoders to radians
Change-Id: I0afb9b27e2627b13902f88a5e61be2af12a104d2
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/wpilib/swerve/BUILD b/frc971/wpilib/swerve/BUILD
index 0b2cc1c..b060520 100644
--- a/frc971/wpilib/swerve/BUILD
+++ b/frc971/wpilib/swerve/BUILD
@@ -1,3 +1,5 @@
+load("//aos/flatbuffers:generate.bzl", "static_flatbuffer")
+
package(default_visibility = ["//visibility:public"])
cc_library(
@@ -27,7 +29,14 @@
"swerve_module.h",
],
deps = [
+ ":swerve_constants_fbs",
"//frc971/wpilib:encoder_and_potentiometer",
"//frc971/wpilib:talonfx",
],
)
+
+static_flatbuffer(
+ name = "swerve_constants_fbs",
+ srcs = ["swerve_constants.fbs"],
+ visibility = ["//visibility:public"],
+)
diff --git a/frc971/wpilib/swerve/swerve_constants.fbs b/frc971/wpilib/swerve/swerve_constants.fbs
new file mode 100644
index 0000000..4d93512
--- /dev/null
+++ b/frc971/wpilib/swerve/swerve_constants.fbs
@@ -0,0 +1,14 @@
+namespace frc971.wpilib.swerve;
+
+// Captures the robot-design-specific constants for a swerve module.
+// I.e., gear ratios and the such but not zeroing constants that vary by
+// instance of the design.
+table SwervePositionConstants {
+ // Scales between the relative and absolute encoders on the rotation
+ // module and the actual rotation of the module, in radians.
+ // module_rotation_in_radians = *_encoder_scale * encoder_reading
+ // The relative encoder reading will generally be the actual count of the
+ // encoder and the absolute encoder will generally vary between 0 and 1.
+ relative_encoder_scale:double (id: 0);
+ absolute_encoder_scale:double (id: 1);
+}
diff --git a/frc971/wpilib/swerve/swerve_module.h b/frc971/wpilib/swerve/swerve_module.h
index 1a16dce..7d91797 100644
--- a/frc971/wpilib/swerve/swerve_module.h
+++ b/frc971/wpilib/swerve/swerve_module.h
@@ -5,6 +5,7 @@
#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/swerve/swerve_constants_static.h"
#include "frc971/wpilib/talonfx.h"
namespace frc971::wpilib::swerve {
@@ -51,11 +52,14 @@
// Populates the Position message with the mag encoder values.
void PopulatePosition(
- frc971::control_loops::swerve::SwerveModulePositionStatic *fbs) {
+ frc971::control_loops::swerve::SwerveModulePositionStatic *fbs,
+ const SwervePositionConstants *constants) {
auto rotation_position = fbs->add_rotation_position();
- rotation_position->set_encoder(rotation_encoder.ReadRelativeEncoder());
+ rotation_position->set_encoder(rotation_encoder.ReadRelativeEncoder() *
+ constants->relative_encoder_scale());
rotation_position->set_absolute_encoder(
- rotation_encoder.ReadAbsoluteEncoder());
+ rotation_encoder.ReadAbsoluteEncoder() *
+ constants->absolute_encoder_scale());
}
// Populates a CAN-position message with the CAN-based devices (currently,
diff --git a/y2024_swerve/BUILD b/y2024_swerve/BUILD
index acc300d..26c3d05 100644
--- a/y2024_swerve/BUILD
+++ b/y2024_swerve/BUILD
@@ -156,6 +156,7 @@
":constants",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/constants:constants_sender_lib",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_can_position_fbs",
"//frc971/control_loops/swerve:swerve_drivetrain_position_fbs",
@@ -164,6 +165,7 @@
"//frc971/wpilib:talonfx",
"//frc971/wpilib:wpilib_robot_base",
"//frc971/wpilib/swerve:swerve_drivetrain_writer",
+ "//y2024_swerve/constants:constants_fbs",
],
)
diff --git a/y2024_swerve/constants/BUILD b/y2024_swerve/constants/BUILD
index 416b10c..6040e5a 100644
--- a/y2024_swerve/constants/BUILD
+++ b/y2024_swerve/constants/BUILD
@@ -56,6 +56,7 @@
"//frc971/control_loops/drivetrain:drivetrain_config_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
+ "//frc971/wpilib/swerve:swerve_constants_fbs",
"//frc971/zeroing:constants_fbs",
],
)
diff --git a/y2024_swerve/constants/common.json b/y2024_swerve/constants/common.json
index 48a4afa..393b7b9 100644
--- a/y2024_swerve/constants/common.json
+++ b/y2024_swerve/constants/common.json
@@ -1,3 +1,7 @@
"common": {
-
-}
\ No newline at end of file
+ "swerve_positions_constants": {
+ {% set pi = 3.141592653589793 %}
+ "relative_encoder_scale": {{ 2.0 * pi / 4096 }},
+ "absolute_encoder_scale": {{ 2.0 * pi }}
+ }
+}
diff --git a/y2024_swerve/constants/constants.fbs b/y2024_swerve/constants/constants.fbs
index a621f3b..88bddf1 100644
--- a/y2024_swerve/constants/constants.fbs
+++ b/y2024_swerve/constants/constants.fbs
@@ -1,5 +1,6 @@
include "frc971/zeroing/constants.fbs";
include "frc971/vision/calibration.fbs";
+include "frc971/wpilib/swerve/swerve_constants.fbs";
namespace y2024_swerve;
@@ -8,7 +9,7 @@
}
table Common {
-
+ swerve_positions_constants:frc971.wpilib.swerve.SwervePositionConstants (id: 0);
}
table RobotConstants {
diff --git a/y2024_swerve/wpilib_interface.cc b/y2024_swerve/wpilib_interface.cc
index e6863b9..3d370d0 100644
--- a/y2024_swerve/wpilib_interface.cc
+++ b/y2024_swerve/wpilib_interface.cc
@@ -4,6 +4,7 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
+#include "frc971/constants/constants_sender_lib.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"
@@ -13,6 +14,7 @@
#include "frc971/wpilib/talonfx.h"
#include "frc971/wpilib/wpilib_robot_base.h"
#include "y2024_swerve/constants.h"
+#include "y2024_swerve/constants/constants_generated.h"
ABSL_FLAG(bool, ctre_diag_server, false,
"If true, enable the diagnostics server for interacting with "
@@ -45,9 +47,11 @@
public:
SensorReader(aos::ShmEventLoop *event_loop,
std::shared_ptr<const constants::Values> values,
+ const Constants *robot_constants,
frc971::wpilib::swerve::SwerveModules modules)
: ::frc971::wpilib::SensorReader(event_loop),
values_(values),
+ robot_constants_(robot_constants),
drivetrain_position_sender_(
event_loop
->MakeSender<frc971::control_loops::swerve::PositionStatic>(
@@ -61,10 +65,17 @@
{
auto builder = drivetrain_position_sender_.MakeStaticBuilder();
- 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());
+ auto swerve_position_constants =
+ robot_constants_->common()->swerve_positions_constants();
+
+ modules_.front_left->PopulatePosition(builder->add_front_left(),
+ swerve_position_constants);
+ modules_.front_right->PopulatePosition(builder->add_front_right(),
+ swerve_position_constants);
+ modules_.back_left->PopulatePosition(builder->add_back_left(),
+ swerve_position_constants);
+ modules_.back_right->PopulatePosition(builder->add_back_right(),
+ swerve_position_constants);
builder.CheckOk(builder.Send());
}
@@ -102,6 +113,8 @@
private:
std::shared_ptr<const constants::Values> values_;
+ const Constants *robot_constants_;
+
aos::Sender<frc971::control_loops::swerve::PositionStatic>
drivetrain_position_sender_;
@@ -121,6 +134,18 @@
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
aos::configuration::ReadConfig("aos_config.json");
+ frc971::constants::WaitForConstants<y2024_swerve::Constants>(
+ &config.message());
+
+ ::aos::ShmEventLoop constant_fetcher_event_loop(&config.message());
+
+ frc971::constants::ConstantsFetcher<Constants> constants_fetcher(
+ &constant_fetcher_event_loop);
+
+ const Constants *robot_constants = &constants_fetcher.constants();
+
+ AddLoop(&constant_fetcher_event_loop);
+
std::vector<ctre::phoenix6::BaseStatusSignal *> signals_registry;
std::vector<std::shared_ptr<TalonFX>> falcons;
@@ -213,7 +238,8 @@
// 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, modules);
+ SensorReader sensor_reader(&sensor_reader_event_loop, values,
+ robot_constants, modules);
sensor_reader.set_front_left_encoder(
make_encoder(3), std::make_unique<frc::DigitalInput>(3));