commit 1f0cc77e73e98c319d11f35028364193d29770ca
author Austin Schuh <austin.linux@gmail.com> Wed Mar 22 20:29:04 2023 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 22 20:29:59 2023 -0700
tree 324c7cc88f01d8dbfd572bdff5f4f39d392060a8
parent 83c2c9c51d9e8db709be3cbab57d4fa27b97528c

Intake even when spitting

We were missing cubes in auto because the 2 second spit timer was long
enough that we were at the next piece already.  Instead, always respect
the request.

Change-Id: I4762373010fc5a69b64fec026f108713a2a30286
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2023/constants.h

diff --git a/y2023/constants.h b/y2023/constants.h
index 8996597..595a694 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -168,7 +168,7 @@
 
   // Game object is spit from end effector for at least this time
   static constexpr std::chrono::milliseconds kExtraSpittingTime() {
-    return std::chrono::seconds(2);
+    return std::chrono::seconds(1);
   }
 
   // if true, tune down all the arm constants for testing.

y2023/control_loops/superstructure/end_effector.cc

diff --git a/y2023/control_loops/superstructure/end_effector.cc b/y2023/control_loops/superstructure/end_effector.cc
index 8ab2f27..444be0d 100644
--- a/y2023/control_loops/superstructure/end_effector.cc
+++ b/y2023/control_loops/superstructure/end_effector.cc
@@ -18,9 +18,9 @@
       beambreak_(false) {}
 
 void EndEffector::RunIteration(
-    const ::aos::monotonic_clock::time_point timestamp, RollerGoal roller_goal,
-    double falcon_current, double cone_position, bool beambreak,
-    double *roller_voltage, bool preloaded_with_cone) {
+    const ::aos::monotonic_clock::time_point timestamp,
+    RollerGoal roller_goal, double falcon_current, double cone_position,
+    bool beambreak, double *roller_voltage, bool preloaded_with_cone) {
   *roller_voltage = 0.0;
 
   constexpr double kMinCurrent = 40.0;
@@ -32,6 +32,7 @@
     switch (state_) {
       case EndEffectorState::IDLE:
       case EndEffectorState::INTAKING:
+        game_piece_ = vision::Class::CONE_UP;
         state_ = EndEffectorState::LOADED;
         break;
       case EndEffectorState::LOADED:
@@ -124,6 +125,12 @@
         // Finished spitting
         state_ = EndEffectorState::IDLE;
         game_piece_ = vision::Class::NONE;
+      } else if (roller_goal == RollerGoal::INTAKE_CONE_UP ||
+                 roller_goal == RollerGoal::INTAKE_CONE_DOWN ||
+                 roller_goal == RollerGoal::INTAKE_CUBE ||
+                 roller_goal == RollerGoal::INTAKE_LAST) {
+        state_ = EndEffectorState::INTAKING;
+        timer_ = timestamp;
       }
 
       break;