commit e735fa82c52b1d2333468e1c4b0b8e47dee72f60
author Niko Sohmers <nikolai@sohmers.com> Fri Feb 02 16:49:02 2024 -0800
committer Niko Sohmers <nikolai@sohmers.com> Sat Feb 03 20:01:34 2024 -0800
tree b3ced1f82aeebba0a5090dfc4b9e4644e702c16a
parent b7a81bf1c69de1e3ea0d233218ffa24827fed598

Add Transfer Roller Superstructure

Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I494a75aab2c206ef0e762e12937d09c32e53e7f7

y2024/control_loops/superstructure/superstructure.cc

diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 366a8db..9a703e3 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -30,6 +30,7 @@
               "/drivetrain")),
       joystick_state_fetcher_(
           event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+      transfer_goal_(TransferRollerGoal::NONE),
       intake_pivot_(
           robot_constants_->common()->intake_pivot(),
           robot_constants_->robot()->intake_constants()->intake_pivot_zero()) {
@@ -70,17 +71,44 @@
       output_struct.intake_roller_voltage = 0.0;
       break;
     case IntakeRollerGoal::SPIT:
+      transfer_goal_ = TransferRollerGoal::TRANSFER_OUT;
       intake_roller_state = IntakeRollerState::SPITTING;
       output_struct.intake_roller_voltage =
           robot_constants_->common()->intake_roller_voltages()->spitting();
       break;
     case IntakeRollerGoal::INTAKE:
+      transfer_goal_ = TransferRollerGoal::TRANSFER_IN;
       intake_roller_state = IntakeRollerState::INTAKING;
       output_struct.intake_roller_voltage =
           robot_constants_->common()->intake_roller_voltages()->intaking();
       break;
   }
 
+  TransferRollerState transfer_roller_state = TransferRollerState::NONE;
+
+  switch (unsafe_goal != nullptr ? transfer_goal_ : TransferRollerGoal::NONE) {
+    case TransferRollerGoal::NONE:
+      output_struct.transfer_roller_voltage = 0.0;
+      break;
+    case TransferRollerGoal::TRANSFER_IN:
+      if (position->transfer_beambreak()) {
+        transfer_goal_ = TransferRollerGoal::NONE;
+        transfer_roller_state = TransferRollerState::NONE;
+        output_struct.transfer_roller_voltage = 0.0;
+        break;
+      }
+      transfer_roller_state = TransferRollerState::TRANSFERING_IN;
+      output_struct.transfer_roller_voltage =
+          robot_constants_->common()->transfer_roller_voltages()->transfer_in();
+      break;
+    case TransferRollerGoal::TRANSFER_OUT:
+      transfer_roller_state = TransferRollerState::TRANSFERING_OUT;
+      output_struct.transfer_roller_voltage = robot_constants_->common()
+                                                  ->transfer_roller_voltages()
+                                                  ->transfer_out();
+      break;
+  }
+
   if (joystick_state_fetcher_.Fetch() &&
       joystick_state_fetcher_->has_alliance()) {
     alliance_ = joystick_state_fetcher_->alliance();
@@ -118,6 +146,7 @@
   status_builder.add_estopped(estopped);
   status_builder.add_intake_roller_state(intake_roller_state);
   status_builder.add_intake_pivot_state(intake_pivot_status_offset);
+  status_builder.add_transfer_roller_state(transfer_roller_state);
 
   (void)status->Send(status_builder.Finish());
 }