commit b0099646e19c1785a772360b6989cb0b7b1bda8b
author Austin Schuh <austin.linux@gmail.com> Sun Apr 03 21:37:27 2016 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 13 22:21:46 2016 -0700
tree 3bace9195bc0bbaee6372eef3ece7824a245b9b9
parent 1c85bc8ad62b7afc0fe5a8a9ae27d2a857dd5d06

Run down compensation when shooting.

Change-Id: I2a179cac93f7989ab037123738585f8e06ca9964

y2016/joystick_reader.cc

diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index b542d04..dcc528f 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -66,6 +66,7 @@
 const ButtonLocation kChevalDeFrise(3, 8);
 
 const ButtonLocation kVisionAlign(3, 4);
+constexpr double kDownOffset = 0.011;
 
 class Reader : public ::aos::input::JoystickInput {
  public:
@@ -122,6 +123,7 @@
 
     const double wheel = -data.GetAxis(kSteeringWheel);
     const double throttle = -data.GetAxis(kDriveThrottle);
+    drivetrain_queue.status.FetchLatest();
 
     if (data.IsPressed(kVisionAlign) && vision_valid_ &&
         !vision_action_running_) {
@@ -143,7 +145,6 @@
     }
 
     if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
-      drivetrain_queue.status.FetchLatest();
       if (drivetrain_queue.status.get()) {
         left_goal = drivetrain_queue.status->estimated_left_position;
         right_goal = drivetrain_queue.status->estimated_right_position;
@@ -213,18 +214,27 @@
       // Forwards shot
       shoulder_goal_ = M_PI / 2.0 + 0.1;
       wrist_goal_ = M_PI + 0.43;
+      if (drivetrain_queue.status.get()) {
+        wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+      }
       shooter_velocity_ = 640.0;
       intake_goal_ = intake_when_shooting;
     } else if (data.IsPressed(kFrontLong)) {
       // Forwards shot
       shoulder_goal_ = M_PI / 2.0 + 0.1;
-      wrist_goal_ = M_PI + 0.40;
+      wrist_goal_ = M_PI + 0.41;
+      if (drivetrain_queue.status.get()) {
+        wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+      }
       shooter_velocity_ = 640.0;
       intake_goal_ = intake_when_shooting;
     } else if (data.IsPressed(kBackLong)) {
       // Backwards shot
       shoulder_goal_ = M_PI / 2.0 - 0.4;
       wrist_goal_ = -0.62;
+      if (drivetrain_queue.status.get()) {
+        wrist_goal_ += drivetrain_queue.status->ground_angle + kDownOffset;
+      }
       shooter_velocity_ = 640.0;
       intake_goal_ = intake_when_shooting;
     } else if (data.IsPressed(kBackFender)) {
@@ -343,7 +353,11 @@
         new_superstructure_goal->max_angular_acceleration_intake = 40.0;
       }
       new_superstructure_goal->max_angular_acceleration_shoulder = 10.0;
-      new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+      if (shoulder_goal_ > 1.0) {
+        new_superstructure_goal->max_angular_acceleration_wrist = 45.0;
+      } else {
+        new_superstructure_goal->max_angular_acceleration_wrist = 25.0;
+      }
 
       // Granny mode
       /*