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;
}