commit b1b2d8ec8ac5be3e56c95ae49fdf79d0dd78719b
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Fri Feb 21 21:11:46 2020 -0800
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Wed Mar 04 20:05:24 2020 -0800
tree 460c225f29a295b2b5aeeb7549c84cabda0f8777
parent 3b3496ad47082d0db923ef55eb821376a3d2be2f

Perform basic turret auto-aiming

Implements a basic aimer which will shoot balls straight at the origin.

This still leaves a variety of things to be done:
-Actually shooting at the correct coordinates.
-Choosing between whether to shoot at the 2-point goal or 3-point goal.
-Managing turret wrap/redundancy intelligently.
-Shooting on the fly.

Change-Id: If4fc6249951faa5300411a0ca7d29da9e11dbb15

y2020/control_loops/superstructure/superstructure.cc

diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 9a6c3cd..d1731e1 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -16,7 +16,9 @@
       hood_(constants::GetValues().hood),
       intake_joint_(constants::GetValues().intake),
       turret_(constants::GetValues().turret.subsystem_params),
-      shooter_() {
+      drivetrain_status_fetcher_(
+          event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+              "/drivetrain")) {
   event_loop->SetRuntimeRealtimePriority(30);
 }
 
@@ -34,6 +36,13 @@
   const aos::monotonic_clock::time_point position_timestamp =
       event_loop()->context().monotonic_event_time;
 
+  if (drivetrain_status_fetcher_.Fetch()) {
+    aimer_.Update(drivetrain_status_fetcher_.get());
+  }
+
+  const flatbuffers::Offset<AimerStatus> aimer_status_offset =
+      aimer_.PopulateStatus(status->fbb());
+
   OutputT output_struct;
 
   flatbuffers::Offset<AbsoluteEncoderProfiledJointStatus> hood_status_offset =
@@ -49,10 +58,14 @@
           output != nullptr ? &(output_struct.intake_joint_voltage) : nullptr,
           status->fbb());
 
+  const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+      *turret_goal = unsafe_goal != nullptr ? (unsafe_goal->turret_tracking()
+                                                   ? aimer_.TurretGoal()
+                                                   : unsafe_goal->turret())
+                                            : nullptr;
   flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
       turret_status_offset = turret_.Iterate(
-          unsafe_goal != nullptr ? unsafe_goal->turret() : nullptr,
-          position->turret(),
+          turret_goal, position->turret(),
           output != nullptr ? &(output_struct.turret_voltage) : nullptr,
           status->fbb());
 
@@ -92,6 +105,7 @@
   status_builder.add_intake(intake_status_offset);
   status_builder.add_turret(turret_status_offset);
   status_builder.add_shooter(shooter_status_offset);
+  status_builder.add_aimer(aimer_status_offset);
 
   status->Send(status_builder.Finish());