Add spline debug plot
Relatively minimal at the moment--just focused on logic for debugging
which spline is running and what splines are planned.
Change-Id: Ic18aba9dd1e65eed173663854b124fe8c6ef353f
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index ea83668..5a185eb 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -82,6 +82,7 @@
"//frc971/control_loops/drivetrain:down_estimator_plotter",
"//frc971/control_loops/drivetrain:drivetrain_plotter",
"//frc971/control_loops/drivetrain:robot_state_plotter",
+ "//frc971/control_loops/drivetrain:spline_plotter",
"//frc971/wpilib:imu_plotter",
"//y2020/control_loops/superstructure:accelerator_plotter",
"//y2020/control_loops/superstructure:finisher_plotter",
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 4bf6c5a..56c99fa 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -24,6 +24,7 @@
import * as proxy from 'org_frc971/aos/network/www/proxy';
import {plotImu} from 'org_frc971/frc971/wpilib/imu_plotter';
import {plotDrivetrain} from 'org_frc971/frc971/control_loops/drivetrain/drivetrain_plotter';
+import {plotSpline} from 'org_frc971/frc971/control_loops/drivetrain/spline_plotter';
import {plotDownEstimator} from 'org_frc971/frc971/control_loops/drivetrain/down_estimator_plotter';
import {plotRobotState} from
'org_frc971/frc971/control_loops/drivetrain/robot_state_plotter'
@@ -90,6 +91,7 @@
['Demo', new PlotState(plotDiv, plotDemo)],
['IMU', new PlotState(plotDiv, plotImu)],
['Drivetrain', new PlotState(plotDiv, plotDrivetrain)],
+ ['Spline Debug', new PlotState(plotDiv, plotSpline)],
['Down Estimator', new PlotState(plotDiv, plotDownEstimator)],
['Robot State', new PlotState(plotDiv, plotRobotState)],
['Finisher', new PlotState(plotDiv, plotFinisher)],
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 4e6f373..d59fc2e 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -787,6 +787,17 @@
)
ts_library(
+ name = "spline_plotter",
+ srcs = ["spline_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
+
+ts_library(
name = "drivetrain_plotter",
srcs = ["drivetrain_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/frc971/control_loops/drivetrain/spline_plotter.ts b/frc971/control_loops/drivetrain/spline_plotter.ts
new file mode 100644
index 0000000..028a3fc
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_plotter.ts
@@ -0,0 +1,68 @@
+// Provides a plot for debugging drivetrain-related issues.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+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;
+
+export function plotSpline(conn: Connection, element: Element): void {
+ const aosPlotter = new AosPlotter(conn);
+
+ const goal = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Goal');
+ const position = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Position');
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+ const output = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Output');
+
+ let currentTop = 0;
+
+ // Polydrivetrain (teleop control) plots
+ const longitudinalPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ longitudinalPlot.plot.getAxisLabels().setTitle('Longitudinal Distance');
+ longitudinalPlot.plot.getAxisLabels().setXLabel(TIME);
+ longitudinalPlot.plot.getAxisLabels().setYLabel('meters');
+
+ longitudinalPlot.addMessageLine(
+ status, ['trajectory_logging', 'distance_remaining']);
+
+ const boolPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ boolPlot.plot.getAxisLabels().setTitle('Bool Flags');
+ boolPlot.plot.getAxisLabels().setXLabel(TIME);
+ boolPlot.plot.getAxisLabels().setYLabel('boolean');
+
+ boolPlot.addMessageLine(status, ['trajectory_logging', 'is_executing'])
+ .setColor(RED);
+ boolPlot.addMessageLine(status, ['trajectory_logging', 'is_executed'])
+ .setColor(BLUE);
+
+ const handlePlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ handlePlot.plot.getAxisLabels().setTitle('Spline Handles');
+ handlePlot.plot.getAxisLabels().setXLabel(TIME);
+ handlePlot.plot.getAxisLabels().setYLabel('handle number');
+
+ handlePlot
+ .addMessageLine(status, ['trajectory_logging', 'available_splines[]'])
+ .setColor(RED)
+ .setDrawLine(false);
+ handlePlot
+ .addMessageLine(status, ['trajectory_logging', 'goal_spline_handle'])
+ .setColor(BLUE)
+ .setPointSize(0.0);
+ handlePlot
+ .addMessageLine(status, ['trajectory_logging', 'current_spline_idx'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+}