Don't disable the roller when intaking continually
Change-Id: I8293b10d717759f05bfe2acb0bdc65c68eca7a65
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 874cfad..c7262bf 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -59,6 +59,12 @@
break;
case EndEffectorState::INTAKING:
// If intaking and beam break is not triggered, keep intaking
+ if (roller_goal == RollerGoal::INTAKE_CONE ||
+ roller_goal == RollerGoal::INTAKE_CUBE ||
+ roller_goal == RollerGoal::INTAKE_LAST) {
+ timer_ = timestamp;
+ }
+
if (beambreak_status) {
// Beam has been broken, switch to loaded.
state_ = EndEffectorState::LOADED;
@@ -77,10 +83,10 @@
break;
case EndEffectorState::LOADED:
+ timer_ = timestamp;
// If loaded and beam break not triggered, intake
if (!beambreak_status) {
state_ = EndEffectorState::INTAKING;
- timer_ = timestamp;
}
break;
case EndEffectorState::SPITTING: