commit 29dcc17101a9243931231a54e063b2a20d9e4c63
author Milind Upadhyay <milind.upadhyay@gmail.com> Sat Apr 02 19:21:30 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sat Apr 02 19:33:53 2022 -0700
tree 19b0fc0820a0c845aa277d34fb99cb6780ff21b4
parent 482ce6c5e04bf0ce5affc06f4588f84a608b7421

Switch back to using one transfer roller voltage

Signed-off-by: Milind Upadhyay <milind.upadhyay@gmail.com>
Change-Id: I42bdfec3813f009aa89066d4682658db956a5a89

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 266ebfa..3d5284e 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -70,8 +70,7 @@
   const CatapultGoal *catapult_goal = nullptr;
   double roller_speed_compensated_front = 0.0;
   double roller_speed_compensated_back = 0.0;
-  double transfer_roller_speed_front = 0.0;
-  double transfer_roller_speed_back = 0.0;
+  double transfer_roller_speed = 0.0;
   double flipper_arms_voltage = 0.0;
   bool have_active_intake_request = false;
 
@@ -84,8 +83,7 @@
         unsafe_goal->roller_speed_back() -
         std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
 
-    transfer_roller_speed_front = unsafe_goal->transfer_roller_speed_front();
-    transfer_roller_speed_back = unsafe_goal->transfer_roller_speed_back();
+    transfer_roller_speed = unsafe_goal->transfer_roller_speed();
 
     turret_goal =
         unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
@@ -175,13 +173,13 @@
   }
 
   // Check if we're either spitting of have lost the ball.
-  if (transfer_roller_speed_front < 0.0 ||
+  if ((transfer_roller_speed < 0.0 && front_intake_has_ball_) ||
       timestamp >
           front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
     front_intake_has_ball_ = false;
   }
 
-  if (transfer_roller_speed_back < 0.0 ||
+  if ((transfer_roller_speed > 0.0 && back_intake_has_ball_) ||
       timestamp >
           back_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
     back_intake_has_ball_ = false;
@@ -194,18 +192,18 @@
   if (front_intake_has_ball_) {
     roller_speed_compensated_front = 0.0;
     if (position->intake_beambreak_front()) {
-      transfer_roller_speed_front = -wiggle_voltage;
+      transfer_roller_speed = -wiggle_voltage;
     } else {
-      transfer_roller_speed_front = wiggle_voltage;
+      transfer_roller_speed = wiggle_voltage;
     }
   }
 
   if (back_intake_has_ball_) {
     roller_speed_compensated_back = 0.0;
     if (position->intake_beambreak_back()) {
-      transfer_roller_speed_back = -wiggle_voltage;
+      transfer_roller_speed = wiggle_voltage;
     } else {
-      transfer_roller_speed_back = wiggle_voltage;
+      transfer_roller_speed = -wiggle_voltage;
     }
   }
 
@@ -296,11 +294,9 @@
 
       // Transfer rollers and flipper arm belt on
       if (turret_intake_state_ == RequestedIntake::kFront) {
-        transfer_roller_speed_front =
-            constants::Values::kTransferRollerVoltage();
+        transfer_roller_speed = constants::Values::kTransferRollerVoltage();
       } else {
-        transfer_roller_speed_back =
-            constants::Values::kTransferRollerVoltage();
+        transfer_roller_speed = -constants::Values::kTransferRollerVoltage();
       }
       flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
 
@@ -495,8 +491,7 @@
   if (output != nullptr) {
     output_struct.roller_voltage_front = roller_speed_compensated_front;
     output_struct.roller_voltage_back = roller_speed_compensated_back;
-    output_struct.transfer_roller_voltage_front = transfer_roller_speed_front;
-    output_struct.transfer_roller_voltage_back = transfer_roller_speed_back;
+    output_struct.transfer_roller_voltage = transfer_roller_speed;
     output_struct.flipper_arms_voltage = flipper_arms_voltage;
 
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));