Dont shoot if robot moving too fast
Signed-off-by: Henry Speiser <henry@speiser.net>
Change-Id: Ifbf086d25647ec2887f0d5825255e098620842a6
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 5dbe7e7..266ebfa 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -244,6 +244,10 @@
.turret_position = turret_.estimated_position(),
.shooting = true});
+ // Dont shoot if the robot is moving faster than this
+ constexpr double kMaxShootSpeed = 1.0;
+ const bool moving_too_fast = std::abs(robot_velocity()) > kMaxShootSpeed;
+
switch (state_) {
case SuperstructureState::IDLE: {
// Only change the turret's goal loading position when idle, to prevent us
@@ -369,8 +373,8 @@
kTurretGoalThreshold;
// Don't open the flippers until the turret's ready: give them as little
- // time to get bumped as possible.
- if (!turret_near_goal || collided) {
+ // time to get bumped as possible. Or moving to fast.
+ if (!turret_near_goal || collided || moving_too_fast) {
break;
}
@@ -527,6 +531,7 @@
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
+ status_builder.add_moving_too_fast(moving_too_fast);
status_builder.add_ready_to_fire(state_ == SuperstructureState::LOADED &&
turret_near_goal && !collided);
status_builder.add_state(state_);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 9288da1..341d315 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -753,7 +753,7 @@
SetEnabled(true);
WaitUntilZeroed();
- SendRobotVelocity(3.0);
+ SendRobotVelocity(1.0);
constexpr double kTurretGoal = 2.0;
{
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 06fddd0..9cf9a5f 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -57,6 +57,8 @@
reseating_in_catapult:bool (id: 13);
// Whether the turret is ready for firing
ready_to_fire:bool (id: 20);
+ // Whether the robot is moving too fast to shoot
+ moving_too_fast:bool (id: 21);
// Whether the catapult was told to fire,
// meaning that the turret and flippers are ready for firing
// and we were asked to fire. Different from fire flag in goal.