Tune new hood
Now that we've got better hardware, remove all the hacks to make the old
hood work and tune it. Much better!
Change-Id: I923cff6c133a23e732b7b593d92a5fa40398a298
diff --git a/y2020/control_loops/superstructure/hood_plotter.ts b/y2020/control_loops/superstructure/hood_plotter.ts
new file mode 100644
index 0000000..6de8883
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood_plotter.ts
@@ -0,0 +1,54 @@
+// Provides a plot for debugging robot state-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+const RED = AosPlotter.RED;
+const GREEN = AosPlotter.GREEN;
+const BLUE = AosPlotter.BLUE;
+const BROWN = AosPlotter.BROWN;
+const PINK = AosPlotter.PINK;
+const CYAN = AosPlotter.CYAN;
+const WHITE = AosPlotter.WHITE;
+
+export function plotHood(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource('/superstructure', 'y2020.control_loops.superstructure.Status');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const positionPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ positionPlot.plot.getAxisLabels().setTitle('Position');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('rad');
+ positionPlot.plot.setDefaultYRange([-0.1, 0.7]);
+
+ positionPlot.addMessageLine(goal, ['hood', 'unsafe_goal']).setColor(BLUE).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'goal_position']).setColor(RED).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'position']).setColor(GREEN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'velocity']).setColor(PINK).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'calculated_velocity']).setColor(BROWN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'estimator_state', 'position']).setColor(CYAN).setPointSize(0.0);
+
+ const voltagePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Volts');
+ voltagePlot.plot.setDefaultYRange([-4.0, 16.0]);
+
+ voltagePlot.addMessageLine(output, ['hood_voltage']).setColor(BLUE).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['hood', 'voltage_error']).setColor(RED).setPointSize(0.0);
+ voltagePlot.addMessageLine(robotState, ['voltage_battery']).setColor(GREEN).setPointSize(0.0);
+}