commit 71e434b4f81f7f24d320bdaf0adcc2dbdae51ce8
author Ravago Jones <ravagojones@gmail.com> Sun Mar 06 14:09:40 2022 -0800
committer Ravago Jones <ravagojones@gmail.com> Sun Mar 06 14:16:33 2022 -0800
tree 60298837b4f8b1d5f59d9608a8e274a07f0b541c
parent aabb4e25147a8b115ed0e2e8a0c07536b9d60cfb

Add states to superstructure_plotter

Change-Id: I25df1f33d767e2d44c6acd94b554b083b07c4fa6
Signed-off-by: Ravago Jones <ravagojones@gmail.com>

y2022/control_loops/superstructure/superstructure_plotter.ts

diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index 6581758..36f0c92 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -20,4 +20,62 @@
   const position = aosPlotter.addMessageSource(
       '/superstructure', 'y2022.control_loops.superstructure.Position');
   const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+  const positionPlot =
+      aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+  positionPlot.plot.getAxisLabels().setTitle('States');
+  positionPlot.plot.getAxisLabels().setXLabel(TIME);
+  positionPlot.plot.getAxisLabels().setYLabel('wonky state units');
+  positionPlot.plot.setDefaultYRange([-1.0, 2.0]);
+
+  positionPlot.addMessageLine(position, ['turret_beambreak'])
+      .setColor(RED)
+      .setPointSize(4.0);
+  positionPlot.addMessageLine(status, ['state'])
+      .setColor(CYAN)
+      .setPointSize(1.0);
+  positionPlot.addMessageLine(status, ['flippers_open'])
+      .setColor(WHITE)
+      .setPointSize(1.0);
+  positionPlot.addMessageLine(status, ['reseating_in_catapult'])
+      .setColor(BLUE)
+      .setPointSize(1.0);
+  positionPlot.addMessageLine(status, ['fire'])
+      .setColor(CYAN)
+      .setPointSize(1.0);
+
+
+  const intakePlot =
+      aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+  intakePlot.plot.getAxisLabels().setTitle('Intake');
+  intakePlot.plot.getAxisLabels().setXLabel(TIME);
+  intakePlot.plot.getAxisLabels().setYLabel('wonky state units');
+  intakePlot.plot.setDefaultYRange([-1.0, 2.0]);
+  intakePlot.addMessageLine(status, ['intake_state'])
+      .setColor(RED)
+      .setPointSize(1.0);
+  intakePlot.addMessageLine(position, ['intake_beambreak_front'])
+      .setColor(GREEN)
+      .setPointSize(4.0);
+  intakePlot.addMessageLine(position, ['intake_beambreak_back'])
+      .setColor(PINK)
+      .setPointSize(1.0);
+
+
+  const otherPlot =
+      aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+  otherPlot.plot.getAxisLabels().setTitle('Position');
+  otherPlot.plot.getAxisLabels().setXLabel(TIME);
+  otherPlot.plot.getAxisLabels().setYLabel('rad');
+  otherPlot.plot.setDefaultYRange([-1.0, 2.0]);
+
+  otherPlot.addMessageLine(status, ['catapult', 'position'])
+      .setColor(PINK)
+      .setPointSize(4.0);
+  otherPlot.addMessageLine(position, ['flipper_arm_left', 'encoder'])
+      .setColor(BLUE)
+      .setPointSize(4.0);
+  otherPlot.addMessageLine(position, ['flipper_arm_right', 'encoder'])
+      .setColor(CYAN)
+      .setPointSize(4.0);
 }