Zero the swerve test drivebase
Update the constants to accommodate zeroing and zero the wheels on the
swerve base.
Change-Id: I448deca4ff1bb776072db631c7327a8cc33fbbc8
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/swerve/BUILD b/frc971/control_loops/swerve/BUILD
index e6a3798..fcd60a1 100644
--- a/frc971/control_loops/swerve/BUILD
+++ b/frc971/control_loops/swerve/BUILD
@@ -5,6 +5,15 @@
package(default_visibility = ["//visibility:public"])
static_flatbuffer(
+ name = "swerve_zeroing_fbs",
+ srcs = ["swerve_zeroing.fbs"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/zeroing:constants_fbs",
+ ],
+)
+
+static_flatbuffer(
name = "swerve_drivetrain_goal_fbs",
srcs = ["swerve_drivetrain_goal.fbs"],
)
diff --git a/frc971/control_loops/swerve/swerve_drivetrain_position.fbs b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
index a5c921a..25e2278 100644
--- a/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
+++ b/frc971/control_loops/swerve/swerve_drivetrain_position.fbs
@@ -4,6 +4,10 @@
table SwerveModulePosition {
// Position of the mag encoder for the rotation of the module.
+ // Zero shall be when the bevel gear on the wheel is on the right side of
+ // the wheel and the wheel is aligned to the X axis of the robot.
+ // Positive rotation is when the wheel is rotated counter-clockwise when
+ // looking at the robot from above (same as positive for robot yaw).
rotation_position: frc971.AbsolutePosition (id: 0);
}
diff --git a/frc971/control_loops/swerve/swerve_zeroing.fbs b/frc971/control_loops/swerve/swerve_zeroing.fbs
new file mode 100644
index 0000000..0a50979
--- /dev/null
+++ b/frc971/control_loops/swerve/swerve_zeroing.fbs
@@ -0,0 +1,12 @@
+include "frc971/zeroing/constants.fbs";
+
+namespace frc971.control_loops.swerve;
+
+table SwerveZeroing {
+ front_left:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 0);
+ front_right:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 1);
+ back_left:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 2);
+ back_right:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 3);
+}
+
+root_type SwerveZeroing;
diff --git a/frc971/zeroing/zeroing.h b/frc971/zeroing/zeroing.h
index 8fe381d..68ef38f 100644
--- a/frc971/zeroing/zeroing.h
+++ b/frc971/zeroing/zeroing.h
@@ -6,6 +6,7 @@
#include <cstdint>
#include <vector>
+#include "absl/log/check.h"
#include "flatbuffers/flatbuffers.h"
#include "frc971/constants.h"
@@ -80,6 +81,7 @@
// by buffer_size.
bool Update(const PositionBuffer &position_buffer, size_t buffer_size,
double zeroing_threshold) {
+ CHECK_LT(0u, buffer_size);
bool moving = true;
Position position(position_buffer);
if (buffered_samples_.size() < buffer_size) {
@@ -103,6 +105,7 @@
}
}
buffered_samples_idx_ = (buffered_samples_idx_ + 1) % buffer_size;
+
return moving;
}
diff --git a/y2024_swerve/constants/7971.json b/y2024_swerve/constants/7971.json
index 644af99..25b73e6 100644
--- a/y2024_swerve/constants/7971.json
+++ b/y2024_swerve/constants/7971.json
@@ -5,10 +5,32 @@
{
"robot": {
- "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
- "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
- "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
- "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "swerve_zeroing": {
+ {% set _ = front_left_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = front_right_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = back_left_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = back_right_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ "front_left": {{ front_left_zero | tojson(indent=2)}},
+ "front_right": {{ front_right_zero | tojson(indent=2)}},
+ "back_left": {{ back_left_zero | tojson(indent=2)}},
+ "back_right": {{ back_right_zero | tojson(indent=2)}}
+ },
"cameras": [
{
"calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
diff --git a/y2024_swerve/constants/9971.json b/y2024_swerve/constants/9971.json
index 644af99..e6a216f 100644
--- a/y2024_swerve/constants/9971.json
+++ b/y2024_swerve/constants/9971.json
@@ -5,10 +5,32 @@
{
"robot": {
- "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
- "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
- "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
- "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "swerve_zeroing": {
+ {% set _ = front_left_zero.update(
+ {
+ "measured_absolute_position" : 1.332417183729859
+ }
+ ) %}
+ {% set _ = front_right_zero.update(
+ {
+ "measured_absolute_position" : 3.4927918912411986
+ }
+ ) %}
+ {% set _ = back_left_zero.update(
+ {
+ "measured_absolute_position" : 3.241233979895719
+ }
+ ) %}
+ {% set _ = back_right_zero.update(
+ {
+ "measured_absolute_position" : 6.25249062612849
+ }
+ ) %}
+ "front_left": {{ front_left_zero | tojson(indent=2)}},
+ "front_right": {{ front_right_zero | tojson(indent=2)}},
+ "back_left": {{ back_left_zero | tojson(indent=2)}},
+ "back_right": {{ back_right_zero | tojson(indent=2)}}
+ },
"cameras": [
{
"calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}
diff --git a/y2024_swerve/constants/BUILD b/y2024_swerve/constants/BUILD
index ec06099..42cff81 100644
--- a/y2024_swerve/constants/BUILD
+++ b/y2024_swerve/constants/BUILD
@@ -54,6 +54,7 @@
deps = [
"//frc971/control_loops:profiled_subsystem_fbs",
"//frc971/control_loops/drivetrain:drivetrain_config_fbs",
+ "//frc971/control_loops/swerve:swerve_zeroing_fbs",
"//frc971/vision:calibration_fbs",
"//frc971/vision:target_map_fbs",
"//frc971/wpilib/swerve:swerve_constants_fbs",
diff --git a/y2024_swerve/constants/common.jinja2 b/y2024_swerve/constants/common.jinja2
index 48b843c..57229fc 100644
--- a/y2024_swerve/constants/common.jinja2
+++ b/y2024_swerve/constants/common.jinja2
@@ -2,44 +2,40 @@
{%
set front_left_zero = {
- "average_filter_size": 0,
+ "average_filter_size": 200,
"one_revolution_distance": 2 * M_PI,
- "measured_absolute_position": 0.76761395509829,
- "zeroing_threshold": 0.0,
- "moving_buffer_size": 0,
- "allowable_encoder_error": 0.0,
+ "zeroing_threshold": 0.0005,
+ "moving_buffer_size": 20,
+ "allowable_encoder_error": 0.9,
}
%}
{%
set front_right_zero = {
- "average_filter_size": 0,
+ "average_filter_size": 200,
"one_revolution_distance": 2 * M_PI,
- "measured_absolute_position": 0.779403958443922,
- "zeroing_threshold": 0.0,
- "moving_buffer_size": 0,
- "allowable_encoder_error": 0.0,
+ "zeroing_threshold": 0.0005,
+ "moving_buffer_size": 20,
+ "allowable_encoder_error": 0.9,
}
%}
{%
set back_left_zero = {
- "average_filter_size": 0,
+ "average_filter_size": 200,
"one_revolution_distance": 2 * M_PI,
- "measured_absolute_position": 0.053439698061417,
- "zeroing_threshold": 0.0,
- "moving_buffer_size": 0,
- "allowable_encoder_error": 0.0,
+ "zeroing_threshold": 0.0005,
+ "moving_buffer_size": 20,
+ "allowable_encoder_error": 0.9,
}
%}
{%
set back_right_zero = {
- "average_filter_size": 0,
+ "average_filter_size": 200,
"one_revolution_distance": 2 * M_PI,
- "measured_absolute_position": 0.719329333121509,
- "zeroing_threshold": 0.0,
- "moving_buffer_size": 0,
- "allowable_encoder_error": 0.0,
+ "zeroing_threshold": 0.0005,
+ "moving_buffer_size": 20,
+ "allowable_encoder_error": 0.9,
}
%}
diff --git a/y2024_swerve/constants/constants.fbs b/y2024_swerve/constants/constants.fbs
index 88bddf1..35b5ec9 100644
--- a/y2024_swerve/constants/constants.fbs
+++ b/y2024_swerve/constants/constants.fbs
@@ -1,6 +1,8 @@
include "frc971/zeroing/constants.fbs";
+include "frc971/control_loops/swerve/swerve_zeroing.fbs";
include "frc971/vision/calibration.fbs";
include "frc971/wpilib/swerve/swerve_constants.fbs";
+include "frc971/control_loops/profiled_subsystem.fbs";
namespace y2024_swerve;
@@ -9,15 +11,13 @@
}
table Common {
- swerve_positions_constants:frc971.wpilib.swerve.SwervePositionConstants (id: 0);
+ rotation:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 0);
+ swerve_positions_constants:frc971.wpilib.swerve.SwervePositionConstants (id: 1);
}
table RobotConstants {
- front_left_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 0);
- front_right_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 1);
- back_left_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 2);
- back_right_zeroing_constants:frc971.zeroing.ContinuousAbsoluteEncoderZeroingConstants (id: 3);
- cameras:[CameraConfiguration] (id: 4);
+ swerve_zeroing:frc971.control_loops.swerve.SwerveZeroing (id: 0);
+ cameras:[CameraConfiguration] (id: 1);
}
table Constants{
diff --git a/y2024_swerve/constants/test_data/test_team.json b/y2024_swerve/constants/test_data/test_team.json
index 644af99..25b73e6 100644
--- a/y2024_swerve/constants/test_data/test_team.json
+++ b/y2024_swerve/constants/test_data/test_team.json
@@ -5,10 +5,32 @@
{
"robot": {
- "front_left_zeroing_constants": {{ front_left_zero | tojson(indent=2)}},
- "front_right_zeroing_constants": {{ front_right_zero | tojson(indent=2)}},
- "back_left_zeroing_constants": {{ back_left_zero | tojson(indent=2)}},
- "back_right_zeroing_constants": {{ back_right_zero | tojson(indent=2)}},
+ "swerve_zeroing": {
+ {% set _ = front_left_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = front_right_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = back_left_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ {% set _ = back_right_zero.update(
+ {
+ "measured_absolute_position" : 0.183060873152545
+ }
+ ) %}
+ "front_left": {{ front_left_zero | tojson(indent=2)}},
+ "front_right": {{ front_right_zero | tojson(indent=2)}},
+ "back_left": {{ back_left_zero | tojson(indent=2)}},
+ "back_right": {{ back_right_zero | tojson(indent=2)}}
+ },
"cameras": [
{
"calibration": {% include 'y2024_swerve/constants/calib_files/calibration_imu-9971-0_cam-24-10_2024-02-24_16-44-05.json' %}