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/frc971/analysis/BUILD b/frc971/analysis/BUILD
index c896ce6..9c236c8 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -31,6 +31,7 @@
"//y2023/control_loops/superstructure:superstructure_plotter",
"//y2023/localizer:corrections_plotter",
"//y2023/localizer:localizer_plotter",
+ "//y2024/control_loops/superstructure:superstructure_plotter",
"//y2024/localizer:corrections_plotter",
"//y2024/localizer:localizer_plotter",
],
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 455c7e9..610ac4b 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -21,50 +21,32 @@
// using JSON rather than requiring people to write a script just to create
// a plot.
import {Configuration} from '../../aos/configuration_generated';
-import {Connection} from '../../aos/network/www/proxy';
-import {plotImu} from '../wpilib/imu_plotter';
-import {plotDrivetrain} from '../control_loops/drivetrain/drivetrain_plotter';
-import {plotSpline} from '../control_loops/drivetrain/spline_plotter';
-import {plotDownEstimator} from '../control_loops/drivetrain/down_estimator_plotter';
-import {plotRobotState} from
- '../control_loops/drivetrain/robot_state_plotter'
-import {plotFinisher as plot2020Finisher} from
- '../../y2020/control_loops/superstructure/finisher_plotter'
-import {plotTurret as plot2020Turret} from
- '../../y2020/control_loops/superstructure/turret_plotter'
-import {plotLocalizer as plot2020Localizer} from
- '../../y2020/control_loops/drivetrain/localizer_plotter'
-import {plotAccelerator as plot2020Accelerator} from
- '../../y2020/control_loops/superstructure/accelerator_plotter'
-import {plotHood as plot2020Hood} from
- '../../y2020/control_loops/superstructure/hood_plotter'
-import {plotSuperstructure as plot2021Superstructure} from
- '../../y2021_bot3/control_loops/superstructure/superstructure_plotter';
-import {plotTurret as plot2022Turret} from
- '../../y2022/control_loops/superstructure/turret_plotter'
-import {plotSuperstructure as plot2022Superstructure} from
- '../../y2022/control_loops/superstructure/superstructure_plotter'
-import {plotSuperstructure as plot2023Superstructure} from
- '../../y2023/control_loops/superstructure/superstructure_plotter'
-import {plotCatapult as plot2022Catapult} from
- '../../y2022/control_loops/superstructure/catapult_plotter'
-import {plotIntakeFront as plot2022IntakeFront, plotIntakeBack as plot2022IntakeBack} from
- '../../y2022/control_loops/superstructure/intake_plotter'
-import {plotClimber as plot2022Climber} from
- '../../y2022/control_loops/superstructure/climber_plotter'
-import {plotLocalizer as plot2022Localizer} from
- '../../y2022/localizer/localizer_plotter'
-import {plotLocalizer as plot2023Localizer} from
- '../../y2023/localizer/localizer_plotter'
-import {plotLocalizer as plot2024Localizer} from
- '../../y2024/localizer/localizer_plotter'
-import {plotVision as plot2022Vision} from
- '../../y2022/vision/vision_plotter'
-import {plotVision as plot2023Corrections} from
- '../../y2023/localizer/corrections_plotter'
-import {plotVision as plot2024Corrections} from
- '../../y2024/localizer/corrections_plotter'
import {plotDemo} from '../../aos/network/www/demo_plot';
+import {Connection} from '../../aos/network/www/proxy';
+import {plotLocalizer as plot2020Localizer} from '../../y2020/control_loops/drivetrain/localizer_plotter'
+import {plotAccelerator as plot2020Accelerator} from '../../y2020/control_loops/superstructure/accelerator_plotter'
+import {plotFinisher as plot2020Finisher} from '../../y2020/control_loops/superstructure/finisher_plotter'
+import {plotHood as plot2020Hood} from '../../y2020/control_loops/superstructure/hood_plotter'
+import {plotTurret as plot2020Turret} from '../../y2020/control_loops/superstructure/turret_plotter'
+import {plotSuperstructure as plot2021Superstructure} from '../../y2021_bot3/control_loops/superstructure/superstructure_plotter';
+import {plotCatapult as plot2022Catapult} from '../../y2022/control_loops/superstructure/catapult_plotter'
+import {plotClimber as plot2022Climber} from '../../y2022/control_loops/superstructure/climber_plotter'
+import {plotIntakeBack as plot2022IntakeBack, plotIntakeFront as plot2022IntakeFront} from '../../y2022/control_loops/superstructure/intake_plotter'
+import {plotSuperstructure as plot2022Superstructure} from '../../y2022/control_loops/superstructure/superstructure_plotter'
+import {plotTurret as plot2022Turret} from '../../y2022/control_loops/superstructure/turret_plotter'
+import {plotLocalizer as plot2022Localizer} from '../../y2022/localizer/localizer_plotter'
+import {plotVision as plot2022Vision} from '../../y2022/vision/vision_plotter'
+import {plotSuperstructure as plot2023Superstructure} from '../../y2023/control_loops/superstructure/superstructure_plotter'
+import {plotVision as plot2023Corrections} from '../../y2023/localizer/corrections_plotter'
+import {plotLocalizer as plot2023Localizer} from '../../y2023/localizer/localizer_plotter'
+import {plotClimber as plot2024Climber, plotIntake as plot2024Intake, plotSuperstructure as plot2024Superstructure} from '../../y2024/control_loops/superstructure/superstructure_plotter'
+import {plotVision as plot2024Corrections} from '../../y2024/localizer/corrections_plotter'
+import {plotLocalizer as plot2024Localizer} from '../../y2024/localizer/localizer_plotter'
+import {plotDownEstimator} from '../control_loops/drivetrain/down_estimator_plotter';
+import {plotDrivetrain} from '../control_loops/drivetrain/drivetrain_plotter';
+import {plotRobotState} from '../control_loops/drivetrain/robot_state_plotter'
+import {plotSpline} from '../control_loops/drivetrain/spline_plotter';
+import {plotImu} from '../wpilib/imu_plotter';
const rootDiv = document.createElement('div');
rootDiv.style.width = '100%';
@@ -124,6 +106,9 @@
['Robot State', new PlotState(plotDiv, plotRobotState)],
['2024 Vision', new PlotState(plotDiv, plot2024Corrections)],
['2024 Localizer', new PlotState(plotDiv, plot2024Localizer)],
+ ['2024 Superstructure', new PlotState(plotDiv, plot2024Superstructure)],
+ ['2024 Climber', new PlotState(plotDiv, plot2024Climber)],
+ ['2024 Intake', new PlotState(plotDiv, plot2024Intake)],
['2023 Vision', new PlotState(plotDiv, plot2023Corrections)],
['2023 Localizer', new PlotState(plotDiv, plot2023Localizer)],
['2023 Superstructure', new PlotState(plotDiv, plot2023Superstructure)],
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');
}