Add a couple of superstructure control loop plots
This adds some somewhat generic plots for SZSDOFS's,
creating climber & intake plots for demonstration.
Change-Id: I43c39c2d33c38c6b9ae2d9a820e8d977ddd52eeb
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/superstructure_plotter.ts b/y2024/control_loops/superstructure/superstructure_plotter.ts
index 3498f77..3a2dcaa 100644
--- a/y2024/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2024/control_loops/superstructure/superstructure_plotter.ts
@@ -1,5 +1,5 @@
// Provides a plot for debugging robot state-related issues.
-import {AosPlotter} from '../../../aos/network/www/aos_plotter';
+import {AosPlotter, MessageHandler} from '../../../aos/network/www/aos_plotter';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from '../../../aos/network/www/colors';
import * as proxy from '../../../aos/network/www/proxy';
@@ -7,24 +7,214 @@
const TIME = AosPlotter.TIME;
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
-const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 1;
+
+function plotSzsdofSubsystem(
+ name: string, plotter: AosPlotter, element: Element, position: MessageHandler, positionName: string,
+ status: MessageHandler, statusName: string, output: MessageHandler, outputName: string, hasPot:boolean = true): void {
+ {
+ const positionPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ positionPlot.plot.getAxisLabels().setTitle(name + ' Position');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('Position [rad,m]');
+ positionPlot.addMessageLine(position, [positionName, 'encoder'])
+ .setColor(RED);
+ positionPlot.addMessageLine(position, [positionName, 'absolute_encoder'])
+ .setColor(GREEN);
+ if (hasPot) {
+ positionPlot.addMessageLine(position, [positionName, 'pot'])
+ .setColor(BLUE);
+ }
+ positionPlot
+ .addMessageLine(status, [statusName, 'estimator_state', 'position'])
+ .setColor(BROWN);
+ positionPlot.addMessageLine(status, [statusName, 'position'])
+ .setColor(WHITE);
+ }
+ {
+ const statesPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ statesPlot.plot.getAxisLabels().setTitle(name + ' State');
+ statesPlot.plot.getAxisLabels().setXLabel(TIME);
+ statesPlot.plot.getAxisLabels().setYLabel('[bool,ZeroingError]');
+ statesPlot.addMessageLine(status, [statusName, 'estopped']).setColor(RED);
+ statesPlot.addMessageLine(status, [statusName, 'zeroed']).setColor(GREEN);
+ statesPlot
+ .addMessageLine(status, [statusName, 'estimator_state', 'errors[]'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+ }
+ {
+ const positionConvergencePlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ positionConvergencePlot.plot.getAxisLabels().setTitle(name + ' Position Goals');
+ positionConvergencePlot.plot.getAxisLabels().setXLabel(TIME);
+ positionConvergencePlot.plot.getAxisLabels().setYLabel('[rad,m]');
+ positionConvergencePlot.addMessageLine(status, [statusName, 'position'])
+ .setColor(RED);
+ positionConvergencePlot.addMessageLine(status, [statusName, 'goal_position'])
+ .setColor(GREEN);
+ positionConvergencePlot
+ .addMessageLine(status, [statusName, 'unprofiled_goal_position'])
+ .setColor(BROWN);
+ }
+ {
+ const velocityConvergencePlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ velocityConvergencePlot.plot.getAxisLabels().setTitle(name + ' Velocity Goals');
+ velocityConvergencePlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityConvergencePlot.plot.getAxisLabels().setYLabel('[rad,m]');
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'velocity'])
+ .setColor(RED);
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'calculated_velocity'])
+ .setColor(RED).setDrawLine(false);
+ velocityConvergencePlot.addMessageLine(status, [statusName, 'goal_velocity'])
+ .setColor(GREEN);
+ velocityConvergencePlot
+ .addMessageLine(status, [statusName, 'unprofiled_goal_velocity'])
+ .setColor(BROWN);
+ }
+ {
+ const outputPlot =
+ plotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ outputPlot.plot.getAxisLabels().setTitle(name + ' Outputs');
+ outputPlot.plot.getAxisLabels().setXLabel(TIME);
+ outputPlot.plot.getAxisLabels().setYLabel('[volts]');
+ outputPlot.addMessageLine(output, [outputName])
+ .setColor(RED);
+ outputPlot.addMessageLine(status, [statusName, 'voltage_error'])
+ .setColor(GREEN);
+ outputPlot.addMessageLine(status, [statusName, 'position_power'])
+ .setColor(BLUE);
+ outputPlot.addMessageLine(status, [statusName, 'velocity_power'])
+ .setColor(BROWN);
+ outputPlot.addMessageLine(status, [statusName, 'feedforwards_power'])
+ .setColor(WHITE);
+ }
+}
export function plotSuperstructure(conn: Connection, element: Element): void {
const aosPlotter = new AosPlotter(conn);
- //const goal = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Goal');
- //const output = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Output');
- //const status = aosPlotter.addMessageSource(
- // '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ {
+ const robotStatePlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ robotStatePlot.plot.getAxisLabels().setTitle('Robot State Plot');
+ robotStatePlot.plot.getAxisLabels().setXLabel(TIME);
+ robotStatePlot.plot.getAxisLabels().setYLabel('[bool]');
+ robotStatePlot.addMessageLine(robotState, ['outputs_enabled'])
+ .setColor(RED);
+ robotStatePlot.addMessageLine(status, ['zeroed'])
+ .setColor(GREEN);
+ robotStatePlot.addMessageLine(status, ['estopped'])
+ .setColor(BLUE);
+ }
+}
+
+export function plotClimber(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
const position = aosPlotter.addMessageSource(
'/superstructure', 'y2024.control_loops.superstructure.Position');
- //const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Climber Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['climber_goal']).setColor(RED);
+ }
- 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]);
+ plotSzsdofSubsystem(
+ 'Climber', aosPlotter, element, position, 'climber', status, 'climber',
+ output, 'climber_voltage');
+}
+
+export function plotIntake(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Position');
+
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Intake Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['intake_goal']).setColor(RED);
+ }
+ {
+ const rollerPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ rollerPlot.plot.getAxisLabels().setTitle('Intake Rollers');
+ rollerPlot.plot.getAxisLabels().setXLabel(TIME);
+ rollerPlot.plot.getAxisLabels().setYLabel('[enum,voltage]');
+ rollerPlot.addMessageLine(status, ['intake_roller']).setColor(RED);
+ rollerPlot.addMessageLine(status, ['transfer_roller']).setColor(BLUE);
+ rollerPlot.addMessageLine(output, ['intake_roller_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ rollerPlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BLUE)
+ .setPointSize(0);
+ }
+
+ plotSzsdofSubsystem(
+ 'Intake', aosPlotter, element, position, 'intake_pivot', status, 'intake_pivot',
+ output, 'intake_pivot_voltage', false);
+}
+
+export function plotExtend(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+ const goal = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Goal');
+ const output = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Output');
+ const status = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Status');
+ const position = aosPlotter.addMessageSource(
+ '/superstructure', 'y2024.control_loops.superstructure.Position');
+
+ {
+ const goalPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ goalPlot.plot.getAxisLabels().setTitle('Extend Goal');
+ goalPlot.plot.getAxisLabels().setXLabel(TIME);
+ goalPlot.plot.getAxisLabels().setYLabel('[enum]');
+ goalPlot.addMessageLine(goal, ['intake_goal']).setColor(RED);
+ }
+ {
+ const rollerPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ rollerPlot.plot.getAxisLabels().setTitle('Extend Rollers');
+ rollerPlot.plot.getAxisLabels().setXLabel(TIME);
+ rollerPlot.plot.getAxisLabels().setYLabel('[enum,voltage]');
+ rollerPlot.addMessageLine(status, ['intake_roller']).setColor(RED);
+ rollerPlot.addMessageLine(status, ['transfer_roller']).setColor(BLUE);
+ rollerPlot.addMessageLine(output, ['intake_roller_voltage'])
+ .setColor(RED)
+ .setPointSize(0);
+ rollerPlot.addMessageLine(output, ['transfer_roller_voltage'])
+ .setColor(BLUE)
+ .setPointSize(0);
+ }
+
+ plotSzsdofSubsystem(
+ 'Extend', aosPlotter, element, position, 'extend', status, 'extend',
+ output, 'extend_voltage');
}