Initial tuning/cleanup of 2022 localizer
This is based on some data collection off of the 2020 robot with the IMU
somewhat loosely mounted. Should behave reasonably--when the wheels are
not slipping, this currently tends to be less precise than traditional
methods, but otherwise behaves reasonably, and does handle substantial
wheel slip reasonably.
General changes:
* General tuning.
* Update some 2020 references to refer to 2022.
* Unwrap the encoder readings from the pico board.
* Make use of the pico timestamps.
Next steps are:
* Adding actual connectors for image corrections.
* Making the turret able to aim.
* Tuning this against driver-practice based IMU readings--use TODOs
in the code as a starting point.
Change-Id: Ie3effe2cbb822317f6cd4a201cce939517a4044f
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/y2022/control_loops/localizer/localizer_plotter.ts b/y2022/control_loops/localizer/localizer_plotter.ts
index ce348e7..239d2cb 100644
--- a/y2022/control_loops/localizer/localizer_plotter.ts
+++ b/y2022/control_loops/localizer/localizer_plotter.ts
@@ -22,7 +22,7 @@
const localizer = aosPlotter.addMessageSource(
'/localizer', 'frc971.controls.LocalizerStatus');
const imu = aosPlotter.addRawMessageSource(
- '/drivetrain', 'frc971.IMUValuesBatch',
+ '/localizer', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
// Drivetrain Status estimated relative position
@@ -48,9 +48,21 @@
positionPlot.addMessageLine(position, ['left_encoder'])
.setColor(BROWN)
.setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['left_encoder'])
+ .setColor(BROWN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['left_encoder'])
+ .setColor(RED)
+ .setDrawLine(false);
positionPlot.addMessageLine(position, ['right_encoder'])
.setColor(CYAN)
.setDrawLine(false);
+ positionPlot.addMessageLine(imu, ['right_encoder'])
+ .setColor(CYAN)
+ .setDrawLine(false);
+ positionPlot.addMessageLine(localizer, ['right_encoder'])
+ .setColor(GREEN)
+ .setDrawLine(false);
// Drivetrain Velocities
@@ -167,13 +179,6 @@
accelPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
accelPlot.plot.getAxisLabels().setXLabel('Monotonic Time (sec)');
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_x'])
- .setColor(PINK);
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_y'])
- .setColor(GREEN);
- accelPlot.addMessageLine(localizer, ['no_wheel_status', 'velocity_z'])
- .setColor(BLUE);
-
accelPlot.addMessageLine(localizer, ['model_based', 'accel_state', 'velocity_x'])
.setColor(RED)
.setDrawLine(false);
@@ -197,11 +202,8 @@
xPositionPlot.addMessageLine(status, ['x']).setColor(RED);
xPositionPlot.addMessageLine(status, ['down_estimator', 'position_x'])
.setColor(BLUE);
- xPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'x']).setColor(GREEN);
xPositionPlot.addMessageLine(localizer, ['model_based', 'x']).setColor(CYAN);
- xPositionPlot.plot.setDefaultYRange([0.0, 0.5]);
-
// Absolute Y Position
const yPositionPlot = aosPlotter.addPlot(element);
yPositionPlot.plot.getAxisLabels().setTitle('Y Position');
@@ -212,7 +214,6 @@
localizerY.setColor(RED);
yPositionPlot.addMessageLine(status, ['down_estimator', 'position_y'])
.setColor(BLUE);
- yPositionPlot.addMessageLine(localizer, ['no_wheel_status', 'y']).setColor(GREEN);
yPositionPlot.addMessageLine(localizer, ['model_based', 'y']).setColor(CYAN);
// Gyro
@@ -264,4 +265,13 @@
costPlot.addMessageLine(localizer, ['model_based', 'accel_residual'])
.setColor(CYAN)
.setPointSize(0);
+
+ const timingPlot =
+ aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT]);
+ timingPlot.plot.getAxisLabels().setTitle('Timing');
+ timingPlot.plot.getAxisLabels().setXLabel(TIME);
+
+ timingPlot.addMessageLine(localizer, ['model_based', 'clock_resets'])
+ .setColor(GREEN)
+ .setDrawLine(false);
}