commit b9ba9a5289b54eb0b1b910832e65362346065851
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Thu Mar 31 22:16:01 2022 -0700
committer Austin Schuh <austin.linux@gmail.com> Wed Apr 06 17:09:04 2022 -0700
tree c6547ee8eb4a5f3de230ea93a78cca44bc82b9d9
parent 235e2d9764761a071fd5f2283002be3dd89dc0d6

Add interpolation table for auto-aim speed-over-ground

We actually have a shot velocity that varies significantly with
distance.

Change-Id: I3a4a59ef3cb181c9f437d50f865d04f710a7f961
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>

y2022/control_loops/superstructure/superstructure.cc

diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index b21dd0d..d23e589 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -5,6 +5,9 @@
 #include "frc971/zeroing/wrap.h"
 #include "y2022/control_loops/superstructure/collision_avoidance.h"
 
+DEFINE_bool(ignore_distance, false,
+            "If true, ignore distance when shooting and obay joystick_reader");
+
 namespace y2022 {
 namespace control_loops {
 namespace superstructure {
@@ -28,7 +31,8 @@
           event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
               "/drivetrain")),
       can_position_fetcher_(
-          event_loop->MakeFetcher<CANPosition>("/superstructure")) {
+          event_loop->MakeFetcher<CANPosition>("/superstructure")),
+      aimer_(values) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -92,8 +96,9 @@
 
     constants::Values::ShotParams shot_params;
     const double distance_to_goal = aimer_.DistanceToGoal();
-    if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
-                                       distance_to_goal, &shot_params)) {
+    if (!FLAGS_ignore_distance && unsafe_goal->auto_aim() &&
+        values_->shot_interpolation_table.GetInRange(distance_to_goal,
+                                                     &shot_params)) {
       flatbuffers::FlatBufferBuilder *catapult_goal_fbb =
           catapult_goal_buffer.fbb();
       std::optional<flatbuffers::Offset<
@@ -243,7 +248,7 @@
        .shooting = true});
 
   // Dont shoot if the robot is moving faster than this
-  constexpr double kMaxShootSpeed = 1.7;
+  constexpr double kMaxShootSpeed = 2.7;
   const bool moving_too_fast = std::abs(robot_velocity()) > kMaxShootSpeed;
 
   switch (state_) {