Add more detail to y2020 turret plot
Includes some somewhat gratuitous code for calculating derivatives in
typescript, but it's a useful template.
Change-Id: Iea96b382de9b57c3236f078ceecc4239da577af0
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2020/control_loops/superstructure/turret_plotter.ts b/y2020/control_loops/superstructure/turret_plotter.ts
index 3d975d8..948ca97 100644
--- a/y2020/control_loops/superstructure/turret_plotter.ts
+++ b/y2020/control_loops/superstructure/turret_plotter.ts
@@ -1,29 +1,82 @@
// 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 * as configuration from 'org_frc971/aos/configuration_generated';
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Point} from 'org_frc971/aos/network/www/plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
import Connection = proxy.Connection;
+import Schema = configuration.reflection.Schema;
const TIME = AosPlotter.TIME;
const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+class DerivativeMessageHandler extends MessageHandler {
+ // Calculated magnitude of the measured acceleration from the IMU.
+ private acceleration_magnitudes: Point[] = [];
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+ private readScalar(table: Table, fieldName: string): number {
+ return this.parser.readScalar(table, fieldName);
+ }
+
+ // Computes a numerical derivative for a given input.
+ private derivative(input: Point[]): Point[] {
+ const num_measurements = input.length;
+ const results = [];
+ for (let ii = 0; ii < num_measurements - 1; ++ii) {
+ const x0 = input[ii].x;
+ const x1 = input[ii + 1].x;
+ const y0 = input[ii].y;
+ const y1 = input[ii + 1].y;
+ results.push(new Point((x0 + x1) / 2.0, (y1 - y0) / (x1 - x0)));
+ }
+ return results;
+ }
+
+ getField(field: string[]): Point[] {
+ // Any requested input that ends with "_derivative" will get a derivative
+ // calculated for the provided field.
+ const derivative_suffix = "_derivative";
+ const num_fields = field.length;
+ const end_field = field[num_fields - 1];
+ if (end_field.endsWith(derivative_suffix)) {
+ const field_copy = [];
+ for (let ii = 0; ii < num_fields - 1; ++ii) {
+ field_copy.push(field[ii]);
+ }
+ field_copy.push(end_field.slice(0, end_field.length - derivative_suffix.length));
+ return this.derivative(this.getField(field_copy));
+ } else {
+ return super.getField(field);
+ }
+ }
+}
+
export function plotTurret(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 status = aosPlotter.addRawMessageSource(
+ '/superstructure', 'y2020.control_loops.superstructure.Status',
+ new DerivativeMessageHandler(conn.getSchema('y2020.control_loops.superstructure.Status'))
+ );
const pdpValues =
aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
+ const localizerDebug =
+ aosPlotter.addMessageSource('/drivetrain', 'y2020.control_loops.drivetrain.LocalizerDebug');
var currentTop = 0;
const turretPosPlot = aosPlotter.addPlot(
element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
currentTop += DEFAULT_HEIGHT;
- turretPosPlot.plot.getAxisLabels().setTitle('Turret Goal Position');
+ turretPosPlot.plot.getAxisLabels().setTitle('Turret Position');
turretPosPlot.plot.getAxisLabels().setXLabel(TIME);
turretPosPlot.plot.getAxisLabels().setYLabel('rad');
@@ -33,8 +86,39 @@
turretPosPlot.addMessageLine(status, ['turret', 'position'])
.setColor(GREEN)
.setPointSize(0.0);
+ turretPosPlot.addMessageLine(localizerDebug, ['matches[]', 'implied_turret_goal'])
+ .setColor(GREEN)
+ .setDrawLine(false);
turretPosPlot.addMessageLine(status, ['turret', 'unprofiled_goal_position'])
.setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretVelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretVelPlot.plot.getAxisLabels().setTitle('Turret Velocity');
+ turretVelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretVelPlot.plot.getAxisLabels().setYLabel('rad / sec');
+
+ turretVelPlot.addMessageLine(status, ['aimer', 'turret_velocity'])
+ .setColor(RED)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'velocity'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVelPlot.addMessageLine(status, ['turret', 'unprofiled_goal_velocity'])
+ .setColor(BLUE)
+ .setDrawLine(false);
+
+ const turretAccelPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ turretAccelPlot.plot.getAxisLabels().setTitle('Turret Acceleration');
+ turretAccelPlot.plot.getAxisLabels().setXLabel(TIME);
+ turretAccelPlot.plot.getAxisLabels().setYLabel('rad / sec / sec');
+
+ turretAccelPlot.addMessageLine(status, ['aimer', 'turret_velocity_derivative'])
+ .setColor(RED)
.setPointSize(0.0);
const turretVoltagePlot = aosPlotter.addPlot(
@@ -44,6 +128,15 @@
turretVoltagePlot.plot.getAxisLabels().setXLabel(TIME);
turretVoltagePlot.plot.getAxisLabels().setYLabel('V');
+ turretVoltagePlot.addMessageLine(status, ['turret', 'voltage_error'])
+ .setColor(GREEN)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'position_power'])
+ .setColor(BLUE)
+ .setPointSize(0.0);
+ turretVoltagePlot.addMessageLine(status, ['turret', 'velocity_power'])
+ .setColor(CYAN)
+ .setPointSize(0.0);
turretVoltagePlot.addMessageLine(output, ['turret_voltage'])
.setColor(RED)
.setPointSize(0.0);
@@ -54,7 +147,7 @@
currentPlot.plot.getAxisLabels().setTitle('Current');
currentPlot.plot.getAxisLabels().setXLabel(TIME);
currentPlot.plot.getAxisLabels().setYLabel('Amps');
- currentPlot.plot.setDefaultYRange([0.0, 80.0]);
+ currentPlot.plot.setDefaultYRange([0.0, 40.0]);
currentPlot.addMessageLine(pdpValues, ['currents[6]'])
.setColor(GREEN)
@@ -83,4 +176,16 @@
targetChoicePlot.addMessageLine(status, ['aimer', 'aiming_for_inner_port'])
.setColor(RED)
.setPointSize(0.0);
+
+ const imageAcceptedPlot = aosPlotter.addPlot(
+ element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
+ imageAcceptedPlot.plot.getAxisLabels().setTitle('Image Acceptance');
+ imageAcceptedPlot.plot.getAxisLabels().setXLabel(TIME);
+ imageAcceptedPlot.plot.getAxisLabels().setYLabel('[bool]');
+ imageAcceptedPlot.plot.setDefaultYRange([-0.05, 1.05]);
+
+ imageAcceptedPlot.addMessageLine(localizerDebug, ['matches[]', 'accepted'])
+ .setColor(RED)
+ .setDrawLine(false);
}