Don't disable drivetrain outputs on missing IMU
This also makes it so that the down-estimator doesn't assume that the
2024 robot exists in simulation....
Change-Id: I308f3d88fc0956766a5d46c7438331aeec00b754
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 81f1d8d..242495f 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -579,9 +579,7 @@
// If the filters aren't ready/valid, then disable all outputs (currently,
// this only happens if the IMU is faulted or has not zeroed).
- // TODO(james): Add exceptions so that during competitive play the driver
- // can retain minimal control of the robot.
- if (!filters_.Ready()) {
+ if (dt_config_.require_imu_for_output && !filters_.Ready()) {
output_struct.left_voltage = 0.0;
output_struct.right_voltage = 0.0;
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.fbs b/frc971/control_loops/drivetrain/drivetrain_config.fbs
index 0947063..3d03fb0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_config.fbs
@@ -148,4 +148,5 @@
top_button_use:PistolTopButtonUse = kShift (id: 17);
second_button_use:PistolSecondButtonUse = kShiftLow (id: 18);
bottom_button_use:PistolBottomButtonUse = kSlowDown (id: 19);
+ require_imu_for_output:bool = true (id: 21);
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index 3f56082..4c35b6a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -154,6 +154,9 @@
SplineFollowerConfig spline_follower_config{};
+ // If set, then the IMU must be zeroed before we will send any outputs.
+ bool require_imu_for_output = true;
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {
@@ -261,6 +264,7 @@
ASSIGN(bottom_button_use),
.spline_follower_config = SplineFollowerConfig::FromFlatbuffer(
fbs.spline_follower_config()),
+ ASSIGN(require_imu_for_output),
#undef ASSIGN
};
}
diff --git a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
index 1127d6a..ec794d0 100644
--- a/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
+++ b/y2024/control_loops/drivetrain/drivetrain_config.jinja2.json
@@ -15,7 +15,8 @@
"cols": 3,
"data": [1, 0, 0, 0, 1, 0, 0, 0, 1]
},
- "is_simulated": true,
+ "is_simulated": false,
+ "require_imu_for_output": false,
"down_estimator_config": {
"gravity_threshold": 0.015,
"do_accel_corrections": 1000