Add drivetrain position plot to drivetrain plotter
It is useful to look at the position plot next to the voltage one
when we are debugging the jerkiness of the drivetrain.
Also, Austin found that we have a bug in drivetrain code by looking
at this plot, so if this commit gets merged then we can all use this
plot to find that bug.
Change-Id: I4c3d58c56154f29849ed761a1d85c123cae0dc2f
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index 6538ec8..dd42d20 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -95,6 +95,27 @@
const controllerType = modePlot.addMessageLine(goal, ['controller_type']);
controllerType.setDrawLine(false);
+ // Drivetrain estimated relative position
+ const positionPlot = aosPlotter.addPlot(element, [0, currentTop],
+ [width, height]);
+ currentTop += height;
+ positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
+ "of the Drivetrain");
+ positionPlot.plot.getAxisLabels().setXLabel("Monotonic Time (sec)");
+ positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
+ const leftPosition =
+ positionPlot.addMessageLine(status, ["estimated_left_position"]);
+ leftPosition.setColor(kRed);
+ const rightPosition =
+ positionPlot.addMessageLine(status, ["estimated_right_position"]);
+ rightPosition.setColor(kGreen);
+ const leftPositionGoal =
+ positionPlot.addMessageLine(status, ["profiled_left_position_goal"]);
+ leftPositionGoal.setColor(kBlue);
+ const rightPositionGoal =
+ positionPlot.addMessageLine(status, ["profiled_right_position_goal"]);
+ rightPositionGoal.setColor(kPink);
+
// Drivetrain Output Voltage
const outputPlot =
aosPlotter.addPlot(element, [0, currentTop], [width, height]);