Add Position Encoder and Reader PID Plots
The position encoder plot is useful for debugging drivetrain issues
and the reader pid plot tells us if wpilib interface gets reset.
I created a new file for all robot state plots because there is a
maximum number of plots in one plotting function and the limit
was exceeded.
Change-Id: I489a1a786217ace7ded1a05ce6104205566ba70f
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index f378b68..45e947b 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -96,6 +96,7 @@
"//aos/network/www:demo_plot",
"//aos/network/www:proxy",
"//frc971/control_loops/drivetrain:drivetrain_plotter",
+ "//frc971/control_loops/drivetrain:robot_state_plotter",
"//frc971/wpilib:imu_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 0610f6c..780f944 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -24,6 +24,8 @@
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 {plotRobotState} from
+ 'org_frc971/frc971/control_loops/drivetrain/robot_state_plotter'
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -81,6 +83,7 @@
['Demo', new PlotState(plotDiv, plotDemo)],
['IMU', new PlotState(plotDiv, plotImu)],
['Drivetrain', new PlotState(plotDiv, plotDrivetrain)],
+ ['Robot State', new PlotState(plotDiv, plotRobotState)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index d42b97c..1aeec18 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -727,3 +727,13 @@
"//frc971/wpilib:imu_plot_utils",
],
)
+
+ts_library(
+ name = "robot_state_plotter",
+ srcs = ["robot_state_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index dd42d20..f1ddd75 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -5,22 +5,23 @@
import Connection = proxy.Connection;
-const kRed = [1, 0, 0];
-const kGreen = [0, 1, 0];
-const kBlue = [0, 0, 1];
-const kBrown = [0.6, 0.3, 0];
-const kPink = [1, 0.3, 1];
-const kCyan = [0.3, 1, 1];
-const kWhite = [1, 1, 1];
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT;
+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 plotDrivetrain(conn: Connection, element: Element): void {
- const width = 900;
- const height = 400;
const aosPlotter = new AosPlotter(conn);
- const joystickState = aosPlotter.addMessageSource('/aos', 'aos.JoystickState');
- const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
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(
@@ -31,351 +32,322 @@
let currentTop = 0;
- // Robot Enabled/Disabled and Mode
- const robotStatePlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
- robotStatePlot.plot.getAxisLabels().setTitle('Robot State');
- robotStatePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
- robotStatePlot.plot.getAxisLabels().setYLabel('bool');
- robotStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
-
- const testMode = robotStatePlot.addMessageLine(joystickState, ['test_mode']);
- testMode.setColor(kBlue);
- testMode.setPointSize(0.0);
- const autoMode = robotStatePlot.addMessageLine(joystickState, ['autonomous']);
- autoMode.setColor(kRed);
- autoMode.setPointSize(0.0);
-
- const brownOut = robotStatePlot.addMessageLine(robotState, ['browned_out']);
- brownOut.setColor(kBrown);
- brownOut.setDrawLine(false);
- const enabled = robotStatePlot.addMessageLine(joystickState, ['enabled']);
- enabled.setColor(kGreen);
- enabled.setDrawLine(false);
-
- // Battery Voltage
- const batteryPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
- batteryPlot.plot.getAxisLabels().setTitle('Battery Voltage');
- batteryPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
- batteryPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
-
- batteryPlot.addMessageLine(robotState, ['voltage_battery']);
-
// Polydrivetrain (teleop control) plots
const teleopPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
teleopPlot.plot.getAxisLabels().setTitle('Drivetrain Teleop Goals');
- teleopPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ teleopPlot.plot.getAxisLabels().setXLabel(TIME);
teleopPlot.plot.getAxisLabels().setYLabel('bool, throttle/wheel values');
teleopPlot.plot.setDefaultYRange([-1.1, 1.1]);
const quickTurn = teleopPlot.addMessageLine(goal, ['quickturn']);
- quickTurn.setColor(kRed);
+ quickTurn.setColor(RED);
const throttle = teleopPlot.addMessageLine(goal, ['throttle']);
- throttle.setColor(kGreen);
+ throttle.setColor(GREEN);
const wheel = teleopPlot.addMessageLine(goal, ['wheel']);
- wheel.setColor(kBlue);
+ wheel.setColor(BLUE);
// Drivetrain Control Mode
const modePlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
// TODO(james): Actually add enum support.
modePlot.plot.getAxisLabels().setTitle(
'Drivetrain Mode [POLYDRIVE, MOTION_PROFILE, ' +
'SPLINE_FOLLOWER, LINE_FOLLOWER]');
- modePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ modePlot.plot.getAxisLabels().setXLabel(TIME);
modePlot.plot.getAxisLabels().setYLabel('ControllerType');
modePlot.plot.setDefaultYRange([-0.1, 3.1]);
const controllerType = modePlot.addMessageLine(goal, ['controller_type']);
controllerType.setDrawLine(false);
- // Drivetrain estimated relative position
+ // Drivetrain Status estimated relative position
const positionPlot = aosPlotter.addPlot(element, [0, currentTop],
- [width, height]);
- currentTop += height;
+ [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
positionPlot.plot.getAxisLabels().setTitle("Estimated Relative Position " +
"of the Drivetrain");
- positionPlot.plot.getAxisLabels().setXLabel("Monotonic Time (sec)");
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
positionPlot.plot.getAxisLabels().setYLabel("Relative Position (m)");
const leftPosition =
positionPlot.addMessageLine(status, ["estimated_left_position"]);
- leftPosition.setColor(kRed);
+ leftPosition.setColor(RED);
const rightPosition =
positionPlot.addMessageLine(status, ["estimated_right_position"]);
- rightPosition.setColor(kGreen);
+ rightPosition.setColor(GREEN);
const leftPositionGoal =
positionPlot.addMessageLine(status, ["profiled_left_position_goal"]);
- leftPositionGoal.setColor(kBlue);
+ leftPositionGoal.setColor(BLUE);
const rightPositionGoal =
positionPlot.addMessageLine(status, ["profiled_right_position_goal"]);
- rightPositionGoal.setColor(kPink);
+ rightPositionGoal.setColor(PINK);
+ const leftEncoder = positionPlot.addMessageLine(position, ["left_encoder"]);
+ leftEncoder.setColor(BROWN);
+ const rightEncoder = positionPlot.addMessageLine(position, ["right_encoder"]);
+ rightEncoder.setColor(CYAN);
// Drivetrain Output Voltage
const outputPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
outputPlot.plot.getAxisLabels().setTitle('Drivetrain Output');
- outputPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ outputPlot.plot.getAxisLabels().setXLabel(TIME);
outputPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
const leftVoltage = outputPlot.addMessageLine(output, ['left_voltage']);
- leftVoltage.setColor(kRed);
+ leftVoltage.setColor(RED);
const rightVoltage = outputPlot.addMessageLine(output, ['right_voltage']);
- rightVoltage.setColor(kGreen);
+ rightVoltage.setColor(GREEN);
// Voltage Errors
const voltageErrors =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
voltageErrors.plot.getAxisLabels().setTitle('Voltage Errors');
- voltageErrors.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ voltageErrors.plot.getAxisLabels().setXLabel(TIME);
voltageErrors.plot.getAxisLabels().setYLabel('Voltage (V)');
const leftVoltageError =
voltageErrors.addMessageLine(status, ['left_voltage_error']);
- leftVoltageError.setColor(kRed);
+ leftVoltageError.setColor(RED);
const rightVoltageError =
voltageErrors.addMessageLine(status, ['right_voltage_error']);
- rightVoltageError.setColor(kGreen);
+ rightVoltageError.setColor(GREEN);
const ekfLeftVoltageError =
voltageErrors.addMessageLine(status, ['localizer', 'left_voltage_error']);
- ekfLeftVoltageError.setColor(kPink);
+ ekfLeftVoltageError.setColor(PINK);
const ekfRightVoltageError = voltageErrors.addMessageLine(
status, ['localizer', 'right_voltage_error']);
- ekfRightVoltageError.setColor(kCyan);
+ ekfRightVoltageError.setColor(CYAN);
// Sundry components of the output voltages
const otherVoltages =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
otherVoltages.plot.getAxisLabels().setTitle('Other Voltage Components');
- otherVoltages.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ otherVoltages.plot.getAxisLabels().setXLabel(TIME);
otherVoltages.plot.getAxisLabels().setYLabel('Voltage (V)');
const ffLeftVoltage = otherVoltages.addMessageLine(
status, ['poly_drive_logging', 'ff_left_voltage']);
- ffLeftVoltage.setColor(kRed);
+ ffLeftVoltage.setColor(RED);
ffLeftVoltage.setPointSize(0);
const ffRightVoltage = otherVoltages.addMessageLine(
status, ['poly_drive_logging', 'ff_right_voltage']);
- ffRightVoltage.setColor(kGreen);
+ ffRightVoltage.setColor(GREEN);
ffRightVoltage.setPointSize(0);
const uncappedLeftVoltage =
otherVoltages.addMessageLine(status, ['uncapped_left_voltage']);
- uncappedLeftVoltage.setColor(kRed);
+ uncappedLeftVoltage.setColor(RED);
uncappedLeftVoltage.setDrawLine(false);
const uncappedRightVoltage =
otherVoltages.addMessageLine(status, ['uncapped_right_voltage']);
- uncappedRightVoltage.setColor(kGreen);
+ uncappedRightVoltage.setColor(GREEN);
uncappedRightVoltage.setDrawLine(false);
// Drivetrain Velocities
const velocityPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
velocityPlot.plot.getAxisLabels().setTitle('Velocity Plots');
- velocityPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
velocityPlot.plot.getAxisLabels().setYLabel('Wheel Velocity (m/s)');
const ssLeftVelocityGoal =
velocityPlot.addMessageLine(status, ['profiled_left_velocity_goal']);
- ssLeftVelocityGoal.setColor(kPink);
+ ssLeftVelocityGoal.setColor(PINK);
ssLeftVelocityGoal.setPointSize(0.0);
const ssRightVelocityGoal =
velocityPlot.addMessageLine(status, ['profiled_right_velocity_goal']);
- ssRightVelocityGoal.setColor(kCyan);
+ ssRightVelocityGoal.setColor(CYAN);
ssRightVelocityGoal.setPointSize(0.0);
const polyLeftVelocity = velocityPlot.addMessageLine(
status, ['poly_drive_logging', 'goal_left_velocity']);
- polyLeftVelocity.setColor(kPink);
+ polyLeftVelocity.setColor(PINK);
polyLeftVelocity.setDrawLine(false);
const polyRightVelocity = velocityPlot.addMessageLine(
status, ['poly_drive_logging', 'goal_right_velocity']);
- polyRightVelocity.setColor(kCyan);
+ polyRightVelocity.setColor(CYAN);
polyRightVelocity.setDrawLine(false);
const splineLeftVelocity = velocityPlot.addMessageLine(
status, ['trajectory_logging', 'left_velocity']);
- splineLeftVelocity.setColor(kRed);
+ splineLeftVelocity.setColor(RED);
splineLeftVelocity.setDrawLine(false);
const splineRightVelocity = velocityPlot.addMessageLine(
status, ['trajectory_logging', 'right_velocity']);
- splineRightVelocity.setColor(kGreen);
+ splineRightVelocity.setColor(GREEN);
splineRightVelocity.setDrawLine(false);
const leftVelocity =
velocityPlot.addMessageLine(status, ['estimated_left_velocity']);
- leftVelocity.setColor(kRed);
+ leftVelocity.setColor(RED);
const rightVelocity =
velocityPlot.addMessageLine(status, ['estimated_right_velocity']);
- rightVelocity.setColor(kGreen);
+ rightVelocity.setColor(GREEN);
const ekfLeftVelocity =
velocityPlot.addMessageLine(status, ['localizer', 'left_velocity']);
- ekfLeftVelocity.setColor(kRed);
+ ekfLeftVelocity.setColor(RED);
ekfLeftVelocity.setPointSize(0.0);
const ekfRightVelocity =
velocityPlot.addMessageLine(status, ['localizer', 'right_velocity']);
- ekfRightVelocity.setColor(kGreen);
+ ekfRightVelocity.setColor(GREEN);
ekfRightVelocity.setPointSize(0.0);
// Heading
- const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ const yawPlot = aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
yawPlot.plot.getAxisLabels().setTitle('Robot Yaw');
- yawPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yawPlot.plot.getAxisLabels().setXLabel(TIME);
yawPlot.plot.getAxisLabels().setYLabel('Yaw (rad)');
const splineYaw =
yawPlot.addMessageLine(status, ['trajectory_logging', 'theta']);
splineYaw.setDrawLine(false);
- splineYaw.setColor(kRed);
+ splineYaw.setColor(RED);
const ekfYaw = yawPlot.addMessageLine(status, ['localizer', 'theta']);
- ekfYaw.setColor(kGreen);
+ ekfYaw.setColor(GREEN);
const downEstimatorYaw =
yawPlot.addMessageLine(status, ['down_estimator', 'yaw']);
- downEstimatorYaw.setColor(kBlue);
+ downEstimatorYaw.setColor(BLUE);
// Pitch/Roll
const orientationPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
orientationPlot.plot.getAxisLabels().setTitle('Orientation');
- orientationPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ orientationPlot.plot.getAxisLabels().setXLabel(TIME);
orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
const roll = orientationPlot.addMessageLine(
status, ['down_estimator', 'lateral_pitch']);
- roll.setColor(kRed);
+ roll.setColor(RED);
roll.setLabel('roll');
const pitch = orientationPlot.addMessageLine(
status, ['down_estimator', 'longitudinal_pitch']);
- pitch.setColor(kGreen);
+ pitch.setColor(GREEN);
pitch.setLabel('pitch');
// Accelerometer/Gravity
const accelPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
accelPlot.plot.getAxisLabels().setTitle('Accelerometer Readings');
accelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
const expectedAccelX =
accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_x']);
- expectedAccelX.setColor(kRed);
+ expectedAccelX.setColor(RED);
expectedAccelX.setPointSize(0);
const expectedAccelY =
accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_y']);
- expectedAccelY.setColor(kGreen);
+ expectedAccelY.setColor(GREEN);
expectedAccelY.setPointSize(0);
const expectedAccelZ =
accelPlot.addMessageLine(status, ['down_estimator', 'expected_accel_z']);
- expectedAccelZ.setColor(kBlue);
+ expectedAccelZ.setColor(BLUE);
expectedAccelZ.setPointSize(0);
const gravity_magnitude =
accelPlot.addMessageLine(status, ['down_estimator', 'gravity_magnitude']);
- gravity_magnitude.setColor(kWhite);
+ gravity_magnitude.setColor(WHITE);
gravity_magnitude.setPointSize(0);
const accelX = accelPlot.addMessageLine(imu, ['accelerometer_x']);
- accelX.setColor(kRed);
+ accelX.setColor(RED);
accelX.setDrawLine(false);
const accelY = accelPlot.addMessageLine(imu, ['accelerometer_y']);
- accelY.setColor(kGreen);
+ accelY.setColor(GREEN);
accelY.setDrawLine(false);
const accelZ = accelPlot.addMessageLine(imu, ['accelerometer_z']);
- accelZ.setColor(kBlue);
+ accelZ.setColor(BLUE);
accelZ.setDrawLine(false);
// Absolute X Position
const xPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
xPositionPlot.plot.getAxisLabels().setTitle('X Position');
- xPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ xPositionPlot.plot.getAxisLabels().setXLabel(TIME);
xPositionPlot.plot.getAxisLabels().setYLabel('X Position (m)');
const localizerX = xPositionPlot.addMessageLine(status, ['x']);
- localizerX.setColor(kRed);
+ localizerX.setColor(RED);
const splineX =
xPositionPlot.addMessageLine(status, ['trajectory_logging', 'x']);
- splineX.setColor(kGreen);
+ splineX.setColor(GREEN);
// Absolute Y Position
const yPositionPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
- yPositionPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ yPositionPlot.plot.getAxisLabels().setXLabel(TIME);
yPositionPlot.plot.getAxisLabels().setYLabel('Y Position (m)');
const localizerY = yPositionPlot.addMessageLine(status, ['y']);
- localizerY.setColor(kRed);
+ localizerY.setColor(RED);
const splineY =
yPositionPlot.addMessageLine(status, ['trajectory_logging', 'y']);
- splineY.setColor(kGreen);
+ splineY.setColor(GREEN);
// Gyro
const gyroPlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height]);
- currentTop += height;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ currentTop += DEFAULT_HEIGHT;
gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
const gyroZeroX =
gyroPlot.addMessageLine(status, ['zeroing', 'gyro_x_average']);
- gyroZeroX.setColor(kRed);
+ gyroZeroX.setColor(RED);
gyroZeroX.setPointSize(0);
gyroZeroX.setLabel('Gyro X Zero');
const gyroZeroY =
gyroPlot.addMessageLine(status, ['zeroing', 'gyro_y_average']);
- gyroZeroY.setColor(kGreen);
+ gyroZeroY.setColor(GREEN);
gyroZeroY.setPointSize(0);
gyroZeroY.setLabel('Gyro Y Zero');
const gyroZeroZ =
gyroPlot.addMessageLine(status, ['zeroing', 'gyro_z_average']);
- gyroZeroZ.setColor(kBlue);
+ gyroZeroZ.setColor(BLUE);
gyroZeroZ.setPointSize(0);
gyroZeroZ.setLabel('Gyro Z Zero');
const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
- gyroX.setColor(kRed);
+ gyroX.setColor(RED);
const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
- gyroY.setColor(kGreen);
+ gyroY.setColor(GREEN);
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
- gyroZ.setColor(kBlue);
+ gyroZ.setColor(BLUE);
// IMU States
const imuStatePlot =
- aosPlotter.addPlot(element, [0, currentTop], [width, height / 2]);
- currentTop += height / 2;
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
imuStatePlot.plot.getAxisLabels().setTitle('IMU State');
- imuStatePlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
+ imuStatePlot.plot.getAxisLabels().setXLabel(TIME);
imuStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
const zeroedLine = imuStatePlot.addMessageLine(status, ['zeroing', 'zeroed']);
- zeroedLine.setColor(kRed);
+ zeroedLine.setColor(RED);
zeroedLine.setDrawLine(false);
zeroedLine.setLabel('IMU Zeroed');
const faultedLine =
imuStatePlot.addMessageLine(status, ['zeroing', 'faulted']);
- faultedLine.setColor(kGreen);
+ faultedLine.setColor(GREEN);
faultedLine.setPointSize(0);
faultedLine.setLabel('IMU Faulted');
}
diff --git a/frc971/control_loops/drivetrain/robot_state_plotter.ts b/frc971/control_loops/drivetrain/robot_state_plotter.ts
new file mode 100644
index 0000000..f42c50b
--- /dev/null
+++ b/frc971/control_loops/drivetrain/robot_state_plotter.ts
@@ -0,0 +1,66 @@
+// 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;
+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 plotRobotState(conn: Connection, element: Element) : void {
+ const aosPlotter = new AosPlotter(conn);
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+ const joystickState = aosPlotter.addMessageSource('/aos', 'aos.JoystickState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const robotStatePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ robotStatePlot.plot.getAxisLabels().setTitle('Robot State');
+ robotStatePlot.plot.getAxisLabels().setXLabel(TIME);
+ robotStatePlot.plot.getAxisLabels().setYLabel('bool');
+ robotStatePlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ const testMode = robotStatePlot.addMessageLine(joystickState, ['test_mode']);
+ testMode.setColor(BLUE);
+ testMode.setPointSize(0.0);
+ const autoMode = robotStatePlot.addMessageLine(joystickState, ['autonomous']);
+ autoMode.setColor(RED);
+ autoMode.setPointSize(0.0);
+
+ const brownOut = robotStatePlot.addMessageLine(robotState, ['browned_out']);
+ brownOut.setColor(BROWN);
+ brownOut.setDrawLine(false);
+ const enabled = robotStatePlot.addMessageLine(joystickState, ['enabled']);
+ enabled.setColor(GREEN);
+ enabled.setDrawLine(false);
+
+ // Battery Voltage
+ const batteryPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ batteryPlot.plot.getAxisLabels().setTitle('Battery Voltage');
+ batteryPlot.plot.getAxisLabels().setXLabel(TIME);
+ batteryPlot.plot.getAxisLabels().setYLabel('Voltage (V)');
+
+ batteryPlot.addMessageLine(robotState, ['voltage_battery']);
+
+ // PID of process reading sensors
+ const readerPidPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ readerPidPlot.plot.getAxisLabels().setTitle("PID of Process Reading Sensors");
+ readerPidPlot.plot.getAxisLabels().setXLabel(TIME);
+ readerPidPlot.plot.getAxisLabels().setYLabel("wpilib_interface Process ID");
+ readerPidPlot.addMessageLine(robotState, ["reader_pid"]);
+}