commit 6e7d8d42f1052d7caf13356895828e89db42bd2a
author milind-u <milind.upadhyay@gmail.com> Wed Apr 06 18:30:43 2022 -0700
committer milind-u <milind.upadhyay@gmail.com> Wed Apr 06 18:30:43 2022 -0700
tree 394cc060473de246d6eed68ac12ec31da4eb0095
parent b9ba9a5289b54eb0b1b910832e65362346065851

Add climber servo code

Used for traversing to high bar.

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

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index d23e589..15f9c2a 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -77,6 +77,7 @@
   double transfer_roller_speed = 0.0;
   double flipper_arms_voltage = 0.0;
   bool have_active_intake_request = false;
+  bool climber_servo = false;
 
   if (unsafe_goal != nullptr) {
     roller_speed_compensated_front =
@@ -89,6 +90,8 @@
 
     transfer_roller_speed = unsafe_goal->transfer_roller_speed();
 
+    climber_servo = unsafe_goal->climber_servo();
+
     turret_goal =
         unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
 
@@ -498,6 +501,13 @@
     output_struct.roller_voltage_back = roller_speed_compensated_back;
     output_struct.transfer_roller_voltage = transfer_roller_speed;
     output_struct.flipper_arms_voltage = flipper_arms_voltage;
+    if (climber_servo) {
+      output_struct.climber_servo_left = 0.0;
+      output_struct.climber_servo_right = 1.0;
+    } else {
+      output_struct.climber_servo_left = 1.0;
+      output_struct.climber_servo_right = 0.0;
+    }
 
     output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
   }