Scale swerve mag encoders to radians
Change-Id: I0afb9b27e2627b13902f88a5e61be2af12a104d2
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
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));