commit 28f9fa10778145b5afe5b591838fbdba3c77ef16
author Austin Schuh <austin.linux@gmail.com> Sun Mar 13 21:33:45 2022 -0700
committer Gerrit Code Review <gerrit@software.frc971.org> Sun Mar 13 21:33:45 2022 -0700
tree 24083bf8fe500f9a2678b9d435da4871b58ee95e
parent 7cba56594ed42682d0d9e02efae5612542290f85
parent c9481b81963450d72938dc2d20e12cee15792538

Merge changes I90bc84d1,Ia55a206d,I7608c9ea

* changes:
  Actually go to the intake requested by the driver
  Update turret limits for new wire harness
  Don't give the profile a velocity

y2022/actors/autonomous_actor.cc

diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index bb488d8..339d7bf 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -332,6 +332,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(
@@ -348,6 +351,7 @@
 }
 
 void AutonomousActor::ExtendFrontIntake() {
+  set_requested_intake(RequestedIntake::kFront);
   set_intake_front_goal(kExtendIntakeGoal);
   set_roller_front_voltage(kIntakeRollerVoltage);
   set_transfer_roller_front_voltage(kRollerVoltage);
@@ -356,6 +360,7 @@
 }
 
 void AutonomousActor::RetractFrontIntake() {
+  set_requested_intake(std::nullopt);
   set_intake_front_goal(kRetractIntakeGoal);
   set_roller_front_voltage(0.0);
   set_transfer_roller_front_voltage(0.0);
@@ -364,6 +369,7 @@
 }
 
 void AutonomousActor::ExtendBackIntake() {
+  set_requested_intake(RequestedIntake::kBack);
   set_intake_back_goal(kExtendIntakeGoal);
   set_roller_back_voltage(kIntakeRollerVoltage);
   set_transfer_roller_back_voltage(kRollerVoltage);
@@ -372,6 +378,7 @@
 }
 
 void AutonomousActor::RetractBackIntake() {
+  set_requested_intake(std::nullopt);
   set_intake_back_goal(kRetractIntakeGoal);
   set_roller_back_voltage(0.0);
   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;
 

y2022/vision/camera_reader.cc

diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index 94b2d96..2fd581a 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -216,7 +216,7 @@
   if (localizer_output_fetcher_.Fetch()) {
     const auto node_name = event_loop_->node()->name()->string_view();
     const size_t pi_number =
-        std::atol(node_name.substr(node_name.size() - 1).data());
+        std::atol(node_name.substr(node_name.size() - 1).data()) - 1;
 
     CHECK(localizer_output_fetcher_->has_led_outputs() &&
           localizer_output_fetcher_->led_outputs()->size() > pi_number);