commit ed177c4498f480b899850e45b56482cd051db230
author James Kuszmaul <jabukuszmaul@gmail.com> Thu Sep 30 18:53:08 2021 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sat Oct 09 08:17:58 2021 -0700
tree 8000c6f3f1cd37ef45d761e0a5c3c331349668ef
parent 2eb54436d5d6541ad2841a62f835dc488ee10d48

Correctly handle turret-focused image corrections in localizer

This makes it so that, by default, we will prioritize making localizer
corrections with the intent of keeping the turret accurate (note that
this will prioritize whatever image target we are using, even if it
isn't the actual shooting target). Currently, it allows a small portion
of correction to creep through to correct for gyro drift, but is on the
order of <1 deg / minute currently. Given the quality of our gyro, this
should be fine.

Change-Id: I0be93ad434f53a0fc34d2fbcd9889368cc592cb0
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>

y2020/control_loops/drivetrain/localizer_plotter.ts

diff --git a/y2020/control_loops/drivetrain/localizer_plotter.ts b/y2020/control_loops/drivetrain/localizer_plotter.ts
index 4de2856..e6729dd 100644
--- a/y2020/control_loops/drivetrain/localizer_plotter.ts
+++ b/y2020/control_loops/drivetrain/localizer_plotter.ts
@@ -22,6 +22,10 @@
       aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
   const imageMatch =
       aosPlotter.addMessageSource('/pi1/camera', 'frc971.vision.sift.ImageMatchResult');
+  const drivetrainStatus = aosPlotter.addMessageSource(
+      '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+  const superstructureStatus = aosPlotter.addMessageSource(
+      '/superstructure', 'y2020.control_loops.superstructure.Status');
 
   var currentTop = 0;
 
@@ -50,6 +54,9 @@
   impliedXPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[3]'])
       .setColor(BLUE)
       .setDrawLine(false);
+  impliedXPlot.addMessageLine(drivetrainStatus, ['x'])
+      .setColor(GREEN)
+      .setLabel('Localizer X');
 
   const impliedYPlot = aosPlotter.addPlot(
       element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
@@ -64,6 +71,9 @@
   impliedYPlot.addMessageLine(imageMatch, ['camera_poses[]', 'field_to_camera', 'data[7]'])
       .setColor(BLUE)
       .setDrawLine(false);
+  impliedYPlot.addMessageLine(drivetrainStatus, ['y'])
+      .setColor(GREEN)
+      .setLabel('Localizer Y');
 
   const impliedHeadingPlot = aosPlotter.addPlot(
       element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
@@ -75,6 +85,9 @@
   impliedHeadingPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_robot_theta'])
       .setColor(RED)
       .setDrawLine(false);
+  impliedHeadingPlot.addMessageLine(drivetrainStatus, ['theta'])
+      .setColor(GREEN)
+      .setLabel('Localizer Theta');
 
   const impliedTurretGoalPlot = aosPlotter.addPlot(
       element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
@@ -86,6 +99,8 @@
   impliedTurretGoalPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
       .setColor(RED)
       .setDrawLine(false);
+  impliedTurretGoalPlot.addMessageLine(superstructureStatus, ['aimer', 'turret_position'])
+      .setColor(GREEN);
 
   const imageTimingPlot = aosPlotter.addPlot(
       element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);