commit aab7e1694b82a1c065f8536a3e07fbadb157bc5c
author Austin Schuh <austin.linux@gmail.com> Wed Mar 13 22:44:58 2019 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 13 22:46:20 2019 -0700
tree ccb9e7785a47fe3bfcb7d74163178b61cf951dca
parent 4be5a21d60100b34f2e305f4245e9df864766bdf

Delay moving the intake back in after grabbing a ball

It was ripping it out of our grip.

Change-Id: If0bdd2c8a015df39fb92cd9ee0fc2d9c43df7da7

y2019/joystick_reader.cc

diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index e435b96..b399afa 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -34,6 +34,8 @@
 using ::aos::input::driver_station::POVLocation;
 using ::aos::events::ProtoTXUdpSocket;
 
+namespace chrono = ::std::chrono;
+
 namespace y2019 {
 namespace input {
 namespace joysticks {
@@ -135,6 +137,8 @@
   }
 
   void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+    ::aos::monotonic_clock::time_point monotonic_now =
+        ::aos::monotonic_clock::now();
     superstructure_queue.position.FetchLatest();
     superstructure_queue.status.FetchLatest();
     if (!superstructure_queue.status.get() ||
@@ -143,6 +147,10 @@
       return;
     }
 
+    if (!superstructure_queue.status->has_piece) {
+      last_not_has_piece_ = monotonic_now;
+    }
+
     auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
 
     if (data.PosEdge(kResetLocalizer)) {
@@ -279,7 +287,8 @@
 
     if (switch_ball_) {
       if (kDoBallOutake ||
-          (kDoBallIntake && !superstructure_queue.status->has_piece)) {
+          (kDoBallIntake &&
+           monotonic_now < last_not_has_piece_ + chrono::milliseconds(100))) {
         new_superstructure_goal->intake.unsafe_goal = 0.959327;
       }
 
@@ -291,7 +300,6 @@
         if (kDoBallOutake) {
           new_superstructure_goal->roller_voltage = -6.0;
         } else {
-          new_superstructure_goal->intake.unsafe_goal = -1.2;
           new_superstructure_goal->roller_voltage = 0.0;
         }
       }
@@ -320,10 +328,10 @@
       LOG(ERROR, "Sending superstructure goal failed.\n");
     }
 
-    auto time_now = ::aos::monotonic_clock::now();
-    if (time_now > last_vision_control_ + ::std::chrono::milliseconds(50)) {
+    if (monotonic_now >
+        last_vision_control_ + ::std::chrono::milliseconds(50)) {
       video_tx_->Send(vision_control_);
-      last_vision_control_ = time_now;
+      last_vision_control_ = monotonic_now;
     }
   }
 
@@ -339,6 +347,10 @@
   ::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
   ::aos::monotonic_clock::time_point last_vision_control_ =
       ::aos::monotonic_clock::time_point::min();
+
+  // Time at which we last did not have a game piece.
+  ::aos::monotonic_clock::time_point last_not_has_piece_ =
+      ::aos::monotonic_clock::time_point::min();
 };
 
 }  // namespace joysticks