commit 98154a24fdcff5ffd826da85a3902ad47c97c8c3
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Apr 03 16:09:29 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Apr 03 18:30:02 2021 -0700
tree 939d2c922287076a95c61ae6539092f4bf05585f
parent 98edd5e301b0469e52d70acb5fb43d4bcaac2ae8

Add shooter interpolation

Includes interpolation values based on testing for at-home challenges
from
https://docs.google.com/document/d/1NR9F-ntlSoqZ9LqDzLjn-c14t8ZrppawCCG7wQy47RU/edit

Change-Id: I094242f43f22da7602f60557a0ec2865a4066875

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index c408f67..91e782e 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -64,11 +64,40 @@
   const flatbuffers::Offset<AimerStatus> aimer_status_offset =
       aimer_.PopulateStatus(status->fbb());
 
+  const double distance_to_goal = aimer_.DistanceToGoal();
+
+  aos::FlatbufferFixedAllocatorArray<
+      frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
+      hood_goal;
+  aos::FlatbufferFixedAllocatorArray<ShooterGoal, 64> shooter_goal;
+
+  constants::Values::ShotParams shot_params;
+  if (constants::GetValues().shot_interpolation_table.GetInRange(
+          distance_to_goal, &shot_params)) {
+    hood_goal.Finish(frc971::control_loops::
+                         CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+                             *hood_goal.fbb(), shot_params.hood_angle));
+
+    shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(),
+                                          shot_params.accelerator_power,
+                                          shot_params.finisher_power));
+  } else {
+    hood_goal.Finish(
+        frc971::control_loops::
+            CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+                *hood_goal.fbb(), constants::GetValues().hood.range.upper));
+
+    shooter_goal.Finish(CreateShooterGoal(*shooter_goal.fbb(), 0.0, 0.0));
+  }
+
   OutputT output_struct;
 
   flatbuffers::Offset<AbsoluteAndAbsoluteEncoderProfiledJointStatus>
       hood_status_offset = hood_.Iterate(
-          unsafe_goal != nullptr ? unsafe_goal->hood() : nullptr,
+          unsafe_goal != nullptr
+              ? (unsafe_goal->hood_tracking() ? &hood_goal.message()
+                                              : unsafe_goal->hood())
+              : nullptr,
           position->hood(),
           output != nullptr ? &(output_struct.hood_voltage) : nullptr,
           status->fbb());
@@ -109,6 +138,7 @@
                                                    ? aimer_.TurretGoal()
                                                    : unsafe_goal->turret())
                                             : nullptr;
+
   flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
       turret_status_offset = turret_.Iterate(
           turret_goal, position->turret(),
@@ -117,7 +147,10 @@
 
   flatbuffers::Offset<ShooterStatus> shooter_status_offset =
       shooter_.RunIteration(
-          unsafe_goal != nullptr ? unsafe_goal->shooter() : nullptr,
+          unsafe_goal != nullptr
+              ? (unsafe_goal->shooter_tracking() ? &shooter_goal.message()
+                                                 : unsafe_goal->shooter())
+              : nullptr,
           position->shooter(), status->fbb(),
           output != nullptr ? &(output_struct) : nullptr, position_timestamp);