commit d58b290aea2a959248b4e06b9649cc4e8a07fc3c
author Austin Schuh <austin.linux@gmail.com> Sun Mar 01 19:28:04 2020 -0800
committer Austin Schuh <austin.linux@gmail.com> Wed Mar 04 21:36:35 2020 -0800
tree 3ff563cc5748cdc7bbbb9659787e4aead92c28f1
parent 11d6846db37fbaf591ab5d98f2bec06fdb33df49

Add setpoint setter

Mechanical really likes being able to tune shots easily...  This is
going to live for a bit.

Change-Id: I1c6ba3ffab2f50647af6fbbcbc64cfd5a913a452

y2020/joystick_reader.cc

diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 026a50b..2049e83 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -13,10 +13,11 @@
 #include "aos/network/team_number.h"
 #include "aos/util/log_interval.h"
 #include "frc971/autonomous/base_autonomous_actor.h"
-#include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/constants.h"
+#include "y2020/control_loops/drivetrain/drivetrain_base.h"
 #include "y2020/control_loops/superstructure/superstructure_goal_generated.h"
 #include "y2020/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2020/setpoint_generated.h"
 
 using aos::input::driver_station::ButtonLocation;
 using aos::input::driver_station::ControlBit;
@@ -57,8 +58,9 @@
         superstructure_goal_sender_(
             event_loop->MakeSender<superstructure::Goal>("/superstructure")),
         superstructure_status_fetcher_(
-            event_loop->MakeFetcher<superstructure::Status>(
-                "/superstructure")) {}
+            event_loop->MakeFetcher<superstructure::Status>("/superstructure")),
+        setpoint_fetcher_(event_loop->MakeFetcher<y2020::joysticks::Setpoint>(
+            "/superstructure")) {}
 
   void AutoEnded() override {
     AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
@@ -71,6 +73,8 @@
       return;
     }
 
+    setpoint_fetcher_.Fetch();
+
     double hood_pos = constants::Values::kHoodRange().middle();
     double intake_pos = -0.89;
     double turret_pos = 0.0;
@@ -84,15 +88,26 @@
     }
 
     if (data.IsPressed(kHood)) {
-      hood_pos = 0.05;
+      hood_pos = 0.45;
+    } else {
+      if (setpoint_fetcher_.get()) {
+        hood_pos = setpoint_fetcher_->hood();
+      } else {
+        hood_pos = 0.58;
+      }
     }
 
     if (data.IsPressed(kShootFast)) {
-      accelerator_speed = 500.0;
-      finisher_speed = 500.0;
+      if (setpoint_fetcher_.get()) {
+        accelerator_speed = setpoint_fetcher_->accelerator();
+        finisher_speed = setpoint_fetcher_->finisher();
+      } else {
+        accelerator_speed = 250.0;
+        finisher_speed = 500.0;
+      }
     } else if (data.IsPressed(kShootSlow)) {
-      accelerator_speed = 30.0;
-      finisher_speed = 30.0;
+      accelerator_speed = 180.0;
+      finisher_speed = 375.0;
     }
 
     if (data.IsPressed(kIntakeExtend)) {
@@ -162,6 +177,8 @@
   ::aos::Sender<superstructure::Goal> superstructure_goal_sender_;
 
   ::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
+
+  ::aos::Fetcher<y2020::joysticks::Setpoint> setpoint_fetcher_;
 };
 
 }  // namespace joysticks