commit d86b02cdd9adc07bc11ddfe0d584ff7839f7bd24
author Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 20:57:46 2022 -0700
committer Milind Upadhyay <milind.upadhyay@gmail.com> Sun Mar 13 20:57:46 2022 -0700
tree bde107a8e87c85b5252b2b4d9a086826f20323e0
parent 4147255028491c44deeb489869cccf6e094e5201

Set requested intake in autonomous

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

y2022/actors/autonomous_actor.cc

diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index bf4fd13..fc02071 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -331,6 +331,9 @@
   superstructure_builder.add_roller_speed_compensation(1.5);
   superstructure_builder.add_roller_speed_front(roller_front_voltage_);
   superstructure_builder.add_roller_speed_back(roller_back_voltage_);
+  if (requested_intake_.has_value()) {
+    superstructure_builder.add_turret_intake(*requested_intake_);
+  }
   superstructure_builder.add_transfer_roller_speed_front(
       transfer_roller_front_voltage_);
   superstructure_builder.add_transfer_roller_speed_back(
@@ -347,6 +350,7 @@
 }
 
 void AutonomousActor::ExtendFrontIntake() {
+  set_requested_intake(RequestedIntake::kFront);
   set_intake_front_goal(kExtendIntakeGoal);
   set_roller_front_voltage(kRollerVoltage);
   set_transfer_roller_front_voltage(kRollerVoltage);
@@ -355,6 +359,7 @@
 }
 
 void AutonomousActor::RetractFrontIntake() {
+  set_requested_intake(std::nullopt);
   set_intake_front_goal(kRetractIntakeGoal);
   set_roller_front_voltage(kRollerVoltage);
   set_transfer_roller_front_voltage(0.0);
@@ -363,6 +368,7 @@
 }
 
 void AutonomousActor::ExtendBackIntake() {
+  set_requested_intake(RequestedIntake::kBack);
   set_intake_back_goal(kExtendIntakeGoal);
   set_roller_back_voltage(kRollerVoltage);
   set_transfer_roller_back_voltage(kRollerVoltage);
@@ -371,6 +377,7 @@
 }
 
 void AutonomousActor::RetractBackIntake() {
+  set_requested_intake(std::nullopt);
   set_intake_back_goal(kRetractIntakeGoal);
   set_roller_back_voltage(kRollerVoltage);
   set_transfer_roller_front_voltage(0.0);

y2022/actors/autonomous_actor.h

diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 3286f10..b7d89bf 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -14,6 +14,7 @@
 namespace y2022 {
 namespace actors {
 
+using control_loops::superstructure::RequestedIntake;
 using frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal;
 
 namespace superstructure = y2022::control_loops::superstructure;
@@ -46,6 +47,9 @@
   void set_transfer_roller_back_voltage(double voltage) {
     transfer_roller_back_voltage_ = voltage;
   }
+  void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
+    requested_intake_ = requested_intake;
+  }
 
   void set_fire_at_will(bool fire) { fire_ = fire; }
   void set_preloaded(bool preloaded) { preloaded_ = preloaded; }
@@ -75,6 +79,7 @@
   double roller_back_voltage_ = 0.0;
   double transfer_roller_front_voltage_ = 0.0;
   double transfer_roller_back_voltage_ = 0.0;
+  std::optional<RequestedIntake> requested_intake_ = std::nullopt;
   bool fire_ = false;
   bool preloaded_ = false;