commit e7e6f327c8703263fe17c33016950435505cde8f
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Sep 25 17:24:00 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sun Sep 26 07:15:16 2021 -0700
tree 4efb21776b4395454be2e8930588b32cf1567604
parent 2d4aeb7b190698414b0c3e4afc1f3e3a76a5abb0

Add turret setting to setpoint_setter

Change-Id: I895e647d3e3ba4c10dd7ea83fdd3f7262e34f25d
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2020/joystick_reader.cc

diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 44d8a42..bb57997 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -101,6 +101,7 @@
 
     double hood_pos = constants::Values::kHoodRange().middle();
     double intake_pos = -0.89;
+    double turret_pos = 0.0;
     float roller_speed = 0.0f;
     float roller_speed_compensation = 0.0f;
     double accelerator_speed = 0.0;
@@ -119,6 +120,12 @@
       }
     }
 
+    if (setpoint_fetcher_.get()) {
+      turret_pos = setpoint_fetcher_->turret();
+    } else {
+      turret_pos = 0.0;
+    }
+
     if (data.IsPressed(kShootFast)) {
       if (setpoint_fetcher_.get()) {
         accelerator_speed = setpoint_fetcher_->accelerator();
@@ -175,7 +182,7 @@
 
       flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
           turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
-              *builder.fbb(), 0.0,
+              *builder.fbb(), turret_pos,
               CreateProfileParameters(*builder.fbb(), 6.0, 20.0));
 
       flatbuffers::Offset<superstructure::ShooterGoal> shooter_offset =