Merge "Making image_capture.py executable."
diff --git a/aos/aos_dump.cc b/aos/aos_dump.cc
index 78df645..45168b2 100644
--- a/aos/aos_dump.cc
+++ b/aos/aos_dump.cc
@@ -95,6 +95,9 @@
[channel, &str_builder, &cli_info, &message_count, &next_send_time](
const aos::Context &context, const void * /*message*/) {
if (context.monotonic_event_time > next_send_time) {
+ if (FLAGS_count > 0 && message_count >= FLAGS_count) {
+ return;
+ }
PrintMessage(channel, context, &str_builder);
++message_count;
next_send_time = context.monotonic_event_time +
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index cc5a356..d23314e 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -269,7 +269,7 @@
const T *operator->() const { return get(); }
// Returns true if this fetcher is valid and connected to a channel.
- operator bool() const { return static_cast<bool>(fetcher_); }
+ bool valid() const { return static_cast<bool>(fetcher_); }
// Copies the current flatbuffer into a FlatbufferVector.
FlatbufferVector<T> CopyFlatBuffer() const {
diff --git a/aos/network/www/aos_plotter.ts b/aos/network/www/aos_plotter.ts
index 59db9d9..270c0d9 100644
--- a/aos/network/www/aos_plotter.ts
+++ b/aos/network/www/aos_plotter.ts
@@ -24,7 +24,7 @@
// the required boilerplate, as well as some extra examples about how to
// add axis labels and the such.
import * as configuration from 'org_frc971/aos/configuration_generated';
-import {Line, Plot} from 'org_frc971/aos/network/www/plotter';
+import {Line, Plot, Point} from 'org_frc971/aos/network/www/plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
import * as reflection from 'org_frc971/aos/network/www/reflection'
@@ -93,7 +93,7 @@
// timestamp but the requested field was not populated.
// If you want to retrieve a single signal from a vector, you can specify it
// as "field_name[index]".
- getField(field: string[]): Float32Array {
+ getField(field: string[]): Point[] {
const fieldName = field[field.length - 1];
const subMessage = field.slice(0, field.length - 1);
const results = [];
@@ -114,26 +114,23 @@
}
const time = this.messages[ii].time;
if (tables.length === 0) {
- results.push(time);
- results.push(NaN);
+ results.push(new Point(time, NaN));
} else {
for (const table of tables) {
const values = this.readField(
table, fieldName, Parser.prototype.readScalar,
Parser.prototype.readVectorOfScalars);
if (values === null) {
- results.push(time);
- results.push(NaN);
+ results.push(new Point(time, NaN));
} else {
for (const value of values) {
- results.push(time);
- results.push((value === null) ? NaN : value);
+ results.push(new Point(time, (value === null) ? NaN : value));
}
}
}
}
}
- return new Float32Array(results);
+ return results;
}
numMessages(): number {
return this.messages.length;
diff --git a/aos/network/www/demo_plot.ts b/aos/network/www/demo_plot.ts
index ebdff7c..cbd133a 100644
--- a/aos/network/www/demo_plot.ts
+++ b/aos/network/www/demo_plot.ts
@@ -13,7 +13,7 @@
// (a) Make use of the AosPlotter to plot a shmem message as a time-series.
// (b) Define your own custom plot with whatever data you want.
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
-import {Plot} from 'org_frc971/aos/network/www/plotter';
+import {Plot, Point} from 'org_frc971/aos/network/www/plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
import Connection = proxy.Connection;
@@ -89,11 +89,9 @@
const points1 = [];
const points2 = [];
for (let ii = 0; ii < NUM_POINTS; ++ii) {
- points1.push(ii);
- points2.push(ii);
- points1.push(Math.sin(ii * 10 / NUM_POINTS));
- points2.push(Math.cos(ii * 10 / NUM_POINTS));
+ points1.push(new Point(ii, Math.sin(ii * 10 / NUM_POINTS)));
+ points2.push(new Point(ii, Math.cos(ii * 10 / NUM_POINTS)));
}
- line1.setPoints(new Float32Array(points1));
- line2.setPoints(new Float32Array(points2));
+ line1.setPoints(points1);
+ line2.setPoints(points2);
}
diff --git a/aos/network/www/plotter.ts b/aos/network/www/plotter.ts
index 21d9834..e56a808 100644
--- a/aos/network/www/plotter.ts
+++ b/aos/network/www/plotter.ts
@@ -29,10 +29,72 @@
});
}
+function subtractVec(a: number[], b: number[]): number[] {
+ return cwiseOp(a, b, (p, q) => {
+ return p - q;
+ });
+}
+
+function multVec(a: number[], b: number[]): number[] {
+ return cwiseOp(a, b, (p, q) => {
+ return p * q;
+ });
+}
+
+function divideVec(a: number[], b: number[]): number[] {
+ return cwiseOp(a, b, (p, q) => {
+ return p / q;
+ });
+}
+
+// Parameters used when scaling the lines to the canvas.
+// If a point in a line is at pos then its position in the canvas space will be
+// scale * pos + offset.
+class ZoomParameters {
+ public scale: number[] = [1.0, 1.0];
+ public offset: number[] = [0.0, 0.0];
+ copy():ZoomParameters {
+ const copy = new ZoomParameters();
+ copy.scale = [this.scale[0], this.scale[1]];
+ copy.offset = [this.offset[0], this.offset[1]];
+ return copy;
+ }
+}
+
+export class Point {
+ constructor(
+ public x: number = 0.0,
+ public y: number = 0.0) {}
+}
+
// Represents a single line within a plot. Handles rendering the line with
// all of its points and the appropriate color/markers/lines.
export class Line {
- private points: Float32Array = new Float32Array([]);
+ // Notes on zoom/precision management:
+ // The adjustedPoints field is the buffert of points (formatted [x0, y0, x1,
+ // y1, ..., xn, yn]) that will be read directly by WebGL and operated on in
+ // the vertex shader. However, WebGL provides relatively minimal guarantess
+ // about the floating point precision available in the shaders (to the point
+ // where even Float32 precision is not guaranteed). As such, we
+ // separately maintain the points vector using javascript number's
+ // (arbitrary-precision ints or double-precision floats). We then periodically
+ // set the baseZoom to be equal to the current desired zoom, calculate the
+ // scaled values directly in typescript, store them in adjustedPoints, and
+ // then just pass an identity transformation to WebGL for the zoom parameters.
+ // When actively zooming, we then just use WebGL to compensate for the offset
+ // between the baseZoom and the desired zoom, taking advantage of WebGL's
+ // performance to handle the high-rate updates but then falling back to
+ // typescript periodically to reset the offsets to avoid precision issues.
+ //
+ // As a practical matter, I've found that even if we were to recalculate
+ // the zoom in typescript on every iteration, the penalty is relatively
+ // minor--we still perform far better than using a non-WebGL canvas. This
+ // suggests that the bulk of the performance advantage from using WebGL for
+ // this use-case lies not in doing the zoom updates in the shaders, but rather
+ // in relying on WebGL to figure out how to drawin the lines/points that we
+ // specify.
+ private adjustedPoints: Float32Array = new Float32Array([]);
+ private points: Point[] = [];
private _drawLine: boolean = true;
private _pointSize: number = 3.0;
private _hasUpdate: boolean = false;
@@ -46,7 +108,7 @@
constructor(
private readonly ctx: WebGLRenderingContext,
private readonly program: WebGLProgram,
- private readonly buffer: WebGLBuffer) {
+ private readonly buffer: WebGLBuffer, private baseZoom: ZoomParameters) {
this.pointAttribLocation = this.ctx.getAttribLocation(this.program, 'apos');
this.colorLocation = this.ctx.getUniformLocation(this.program, 'color');
this.pointSizeLocation =
@@ -106,24 +168,18 @@
// Set the points to render. The points in the line are ordered and should
// be of the format:
// [x1, y1, x2, y2, x3, y3, ...., xN, yN]
- setPoints(points: Float32Array) {
- if (points.length % 2 !== 0) {
- throw new Error("Must have even number of elements in points array.");
- }
- if (points.BYTES_PER_ELEMENT != 4) {
- throw new Error(
- 'Must pass in a Float32Array--actual size was ' +
- points.BYTES_PER_ELEMENT + '.');
- }
+ setPoints(points: Point[]) {
this.points = points;
+ this.adjustedPoints = new Float32Array(points.length * 2);
+ this.updateBaseZoom(this.baseZoom);
this._hasUpdate = true;
this._minValues[0] = Infinity;
this._minValues[1] = Infinity;
this._maxValues[0] = -Infinity;
this._maxValues[1] = -Infinity;
- for (let ii = 0; ii < this.points.length; ii += 2) {
- const x = this.points[ii];
- const y = this.points[ii + 1];
+ for (let ii = 0; ii < this.points.length; ++ii) {
+ const x = this.points[ii].x;
+ const y = this.points[ii].y;
if (isNaN(x) || isNaN(y)) {
continue;
@@ -134,7 +190,7 @@
}
}
- getPoints(): Float32Array {
+ getPoints(): Point[] {
return this.points;
}
@@ -148,6 +204,15 @@
return this._label;
}
+ updateBaseZoom(zoom: ZoomParameters) {
+ this.baseZoom = zoom;
+ for (let ii = 0; ii < this.points.length; ++ii) {
+ const point = this.points[ii];
+ this.adjustedPoints[ii * 2] = point.x * zoom.scale[0] + zoom.offset[0];
+ this.adjustedPoints[ii * 2 + 1] = point.y * zoom.scale[1] + zoom.offset[1];
+ }
+ }
+
// Render the line on the canvas.
draw() {
this._hasUpdate = false;
@@ -160,7 +225,7 @@
// confirm that this.points really is a Float32Array.
this.ctx.bufferData(
this.ctx.ARRAY_BUFFER,
- this.points,
+ this.adjustedPoints,
this.ctx.STATIC_DRAW);
{
const numComponents = 2; // pull out 2 values per iteration
@@ -181,22 +246,14 @@
1.0);
if (this._drawLine) {
- this.ctx.drawArrays(this.ctx.LINE_STRIP, 0, this.points.length / 2);
+ this.ctx.drawArrays(this.ctx.LINE_STRIP, 0, this.points.length);
}
if (this._pointSize > 0.0) {
- this.ctx.drawArrays(this.ctx.POINTS, 0, this.points.length / 2);
+ this.ctx.drawArrays(this.ctx.POINTS, 0, this.points.length);
}
}
}
-// Parameters used when scaling the lines to the canvas.
-// If a point in a line is at pos then its position in the canvas space will be
-// scale * pos + offset.
-class ZoomParameters {
- public scale: number[] = [1.0, 1.0];
- public offset: number[] = [0.0, 0.0];
-}
-
enum MouseButton {
Right,
Middle,
@@ -338,6 +395,7 @@
private vertexBuffer: WebGLBuffer;
private lines: Line[] = [];
private zoom: ZoomParameters = new ZoomParameters();
+ private baseZoom: ZoomParameters = new ZoomParameters();
private zoomUpdated: boolean = true;
// Maximum grid lines to render at once--this is used provide an upper limit
// on the number of Line objects we need to create in order to render the
@@ -362,32 +420,24 @@
this.vertexBuffer = this.ctx.createBuffer();
for (let ii = 0; ii < this.MAX_GRID_LINES; ++ii) {
- this.xGridLines.push(new Line(this.ctx, this.program, this.vertexBuffer));
- this.yGridLines.push(new Line(this.ctx, this.program, this.vertexBuffer));
+ this.xGridLines.push(
+ new Line(this.ctx, this.program, this.vertexBuffer, this.baseZoom));
+ this.yGridLines.push(
+ new Line(this.ctx, this.program, this.vertexBuffer, this.baseZoom));
}
}
- setXGrid(lines: Line[]) {
- this.xGridLines = lines;
- }
-
getZoom(): ZoomParameters {
- return this.zoom;
+ return this.zoom.copy();
}
plotToCanvasCoordinates(plotPos: number[]): number[] {
- return addVec(cwiseOp(plotPos, this.zoom.scale, (a, b) => {
- return a * b;
- }), this.zoom.offset);
+ return addVec(multVec(plotPos, this.zoom.scale), this.zoom.offset);
}
canvasToPlotCoordinates(canvasPos: number[]): number[] {
- return cwiseOp(cwiseOp(canvasPos, this.zoom.offset, (a, b) => {
- return a - b;
- }), this.zoom.scale, (a, b) => {
- return a / b;
- });
+ return divideVec(subtractVec(canvasPos, this.zoom.offset), this.zoom.scale);
}
// Tehse return the max/min rendered points, in plot-space (this is helpful
@@ -405,8 +455,14 @@
}
setZoom(zoom: ZoomParameters) {
+ if (this.zoom.scale[0] == zoom.scale[0] &&
+ this.zoom.scale[1] == zoom.scale[1] &&
+ this.zoom.offset[0] == zoom.offset[0] &&
+ this.zoom.offset[1] == zoom.offset[1]) {
+ return;
+ }
this.zoomUpdated = true;
- this.zoom = zoom;
+ this.zoom = zoom.copy();
}
setXTicks(ticks: number[]): void {
@@ -420,8 +476,8 @@
// Update the grid lines.
updateTicks() {
for (let ii = 0; ii < this.MAX_GRID_LINES; ++ii) {
- this.xGridLines[ii].setPoints(new Float32Array([]));
- this.yGridLines[ii].setPoints(new Float32Array([]));
+ this.xGridLines[ii].setPoints([]);
+ this.yGridLines[ii].setPoints([]);
}
const minValues = this.minVisiblePoint();
@@ -429,8 +485,10 @@
for (let ii = 0; ii < this.xTicks.length; ++ii) {
this.xGridLines[ii].setColor([0.0, 0.0, 0.0]);
- const points = new Float32Array(
- [this.xTicks[ii], minValues[1], this.xTicks[ii], maxValues[1]]);
+ const points = [
+ new Point(this.xTicks[ii], minValues[1]),
+ new Point(this.xTicks[ii], maxValues[1])
+ ];
this.xGridLines[ii].setPointSize(0);
this.xGridLines[ii].setPoints(points);
this.xGridLines[ii].draw();
@@ -438,8 +496,10 @@
for (let ii = 0; ii < this.yTicks.length; ++ii) {
this.yGridLines[ii].setColor([0.0, 0.0, 0.0]);
- const points = new Float32Array(
- [minValues[0], this.yTicks[ii], maxValues[0], this.yTicks[ii]]);
+ const points = [
+ new Point(minValues[0], this.yTicks[ii]),
+ new Point(maxValues[0], this.yTicks[ii])
+ ];
this.yGridLines[ii].setPointSize(0);
this.yGridLines[ii].setPoints(points);
this.yGridLines[ii].draw();
@@ -521,7 +581,8 @@
}
addLine(useColorCycle: boolean = true): Line {
- this.lines.push(new Line(this.ctx, this.program, this.vertexBuffer));
+ this.lines.push(
+ new Line(this.ctx, this.program, this.vertexBuffer, this.baseZoom));
const line = this.lines[this.lines.length - 1];
if (useColorCycle) {
line.setColor(LineDrawer.COLOR_CYCLE[this.colorCycleIndex++]);
@@ -555,10 +616,44 @@
this.ctx.useProgram(this.program);
+ // Check for whether the zoom parameters have changed significantly; if so,
+ // update the base zoom.
+ // These thresholds are somewhat arbitrary.
+ const scaleDiff = divideVec(this.zoom.scale, this.baseZoom.scale);
+ const scaleChanged = scaleDiff[0] < 0.9 || scaleDiff[0] > 1.1 ||
+ scaleDiff[1] < 0.9 || scaleDiff[1] > 1.1;
+ const offsetDiff = subtractVec(this.zoom.offset, this.baseZoom.offset);
+ // Note that offset is in the canvas coordinate frame and so just using
+ // hard-coded constants is fine.
+ const offsetChanged =
+ Math.abs(offsetDiff[0]) > 0.1 || Math.abs(offsetDiff[1]) > 0.1;
+ if (scaleChanged || offsetChanged) {
+ this.baseZoom = this.zoom.copy();
+ for (const line of this.lines) {
+ line.updateBaseZoom(this.baseZoom);
+ }
+ for (const line of this.xGridLines) {
+ line.updateBaseZoom(this.baseZoom);
+ }
+ for (const line of this.yGridLines) {
+ line.updateBaseZoom(this.baseZoom);
+ }
+ }
+
+ // all the points in the lines will be pre-scaled by this.baseZoom, so
+ // we need to remove its effects before passing it in.
+ // zoom.scale * pos + zoom.offset = scale * (baseZoom.scale * pos + baseZoom.offset) + offset
+ // zoom.scale = scale * baseZoom.scale
+ // scale = zoom.scale / baseZoom.scale
+ // zoom.offset = scale * baseZoom.offset + offset
+ // offset = zoom.offset - scale * baseZoom.offset
+ const scale = divideVec(this.zoom.scale, this.baseZoom.scale);
+ const offset =
+ subtractVec(this.zoom.offset, multVec(scale, this.baseZoom.offset));
this.ctx.uniform2f(
- this.scaleLocation, this.zoom.scale[0], this.zoom.scale[1]);
+ this.scaleLocation, scale[0], scale[1]);
this.ctx.uniform2f(
- this.offsetLocation, this.zoom.offset[0], this.zoom.offset[1]);
+ this.offsetLocation, offset[0], offset[1]);
}
}
@@ -904,7 +999,7 @@
const currentPosition = this.mousePlotLocation(event);
this.setZoomCorners(this.rectangleStartPosition, currentPosition);
this.rectangleStartPosition = null;
- this.zoomRectangle.setPoints(new Float32Array([]));
+ this.zoomRectangle.setPoints([]);
}
handleMouseMove(event: MouseEvent) {
@@ -936,17 +1031,16 @@
p0[0] = minVisible[0];
p1[0] = maxVisible[0];
}
- this.zoomRectangle.setPoints(
- new Float32Array([p0[0], p0[1]]
- .concat([p0[0], p1[1]])
- .concat([p1[0], p1[1]])
- .concat([p1[0], p0[1]])
- .concat([p0[0], p0[1]])));
+ this.zoomRectangle.setPoints([
+ new Point(p0[0], p0[1]), new Point(p0[0], p1[1]),
+ new Point(p1[0], p1[1]), new Point(p1[0], p0[1]),
+ new Point(p0[0], p0[1])
+ ]);
} else {
this.finishRectangleZoom(event);
}
} else {
- this.zoomRectangle.setPoints(new Float32Array([]));
+ this.zoomRectangle.setPoints([]);
}
this.lastMousePosition = mouseLocation;
}
@@ -1050,7 +1144,7 @@
// Cancel zoom/pan operations on escape.
plot.lastMousePanPosition = null;
plot.rectangleStartPosition = null;
- plot.zoomRectangle.setPoints(new Float32Array([]));
+ plot.zoomRectangle.setPoints([]);
}
}
}
diff --git a/aos/starter/starter_rpc_lib.cc b/aos/starter/starter_rpc_lib.cc
index 67b3fb2..7b86c0a 100644
--- a/aos/starter/starter_rpc_lib.cc
+++ b/aos/starter/starter_rpc_lib.cc
@@ -30,7 +30,7 @@
const aos::Configuration *config) {
std::string_view app_name = name;
for (const auto app : *config->applications()) {
- if (app->executable_name() != nullptr &&
+ if (app->has_executable_name() &&
app->executable_name()->string_view() == name) {
app_name = app->name()->string_view();
break;
@@ -74,7 +74,7 @@
event_loop.MakeFetcher<aos::starter::Status>("/aos");
initial_status_fetcher.Fetch();
auto initial_status =
- initial_status_fetcher
+ initial_status_fetcher.get()
? FindApplicationStatus(*initial_status_fetcher, name)
: nullptr;
@@ -133,8 +133,9 @@
auto status_fetcher = event_loop.MakeFetcher<aos::starter::Status>("/aos");
status_fetcher.Fetch();
- auto status =
- status_fetcher ? FindApplicationStatus(*status_fetcher, name) : nullptr;
+ auto status = status_fetcher.get()
+ ? FindApplicationStatus(*status_fetcher, name)
+ : nullptr;
return status ? aos::CopyFlatBuffer(status)
: FlatbufferDetachedBuffer<
aos::starter::ApplicationStatus>::Empty();
@@ -147,8 +148,9 @@
auto status_fetcher = event_loop.MakeFetcher<aos::starter::Status>("/aos");
status_fetcher.Fetch();
- return (status_fetcher ? std::make_optional(status_fetcher.CopyFlatBuffer())
- : std::nullopt);
+ return (status_fetcher.get()
+ ? std::make_optional(status_fetcher.CopyFlatBuffer())
+ : std::nullopt);
}
} // namespace starter
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_data_utils.ts b/frc971/analysis/plot_data_utils.ts
index 8d42a4a..e918379 100644
--- a/frc971/analysis/plot_data_utils.ts
+++ b/frc971/analysis/plot_data_utils.ts
@@ -4,7 +4,7 @@
import * as plot_data from 'org_frc971/frc971/analysis/plot_data_generated';
import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
-import {Plot} from 'org_frc971/aos/network/www/plotter';
+import {Plot, Point} from 'org_frc971/aos/network/www/plotter';
import * as proxy from 'org_frc971/aos/network/www/proxy';
import Connection = proxy.Connection;
@@ -79,10 +79,10 @@
if (lineFb.label()) {
line.setLabel(lineFb.label());
}
- const points = new Float32Array(lineFb.pointsLength() * 2);
+ const points = [];
for (let kk = 0; kk < lineFb.pointsLength(); ++kk) {
- points[kk * 2] = lineFb.points(kk).x();
- points[kk * 2 + 1] = lineFb.points(kk).y();
+ const point = lineFb.points(kk);
+ points.push(new Point(point.x(), point.y()));
}
if (lineFb.color()) {
line.setColor(
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);
+}
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index ec2ee9e..8a56d96 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -11,6 +11,7 @@
namespace frc971 {
namespace wpilib {
namespace {
+namespace chrono = std::chrono;
namespace registers {
// Flash memory write count
@@ -262,14 +263,14 @@
reset_->Set(false);
// Datasheet says it needs a 1 us pulse, so make sure we do something in
// between asserting and deasserting.
- std::this_thread::sleep_for(::std::chrono::milliseconds(1));
+ std::this_thread::sleep_for(chrono::milliseconds(1));
reset_->Set(true);
state_ = State::kWaitForReset;
// Datasheet says it takes 193 ms to come out of reset, so give it some
// margin on top of that.
initialize_timer_->Setup(event_loop_->monotonic_now() +
- std::chrono::milliseconds(250));
+ chrono::milliseconds(250));
}
break;
@@ -303,7 +304,7 @@
// Start a sensor self test.
WriteRegister(registers::GLOB_CMD, 1 << 2);
// Datasheet says it takes 14ms, so give it some margin.
- std::this_thread::sleep_for(std::chrono::milliseconds(25));
+ std::this_thread::sleep_for(chrono::milliseconds(25));
// Read DIAG_STAT again, and queue up a read of the first part of the
// autospi data packet.
const uint16_t self_test_diag_stat_value =
@@ -335,6 +336,21 @@
// Finally, enable automatic mode so it starts reading data.
spi_->StartAutoTrigger(*data_ready_, true, false);
+
+ // We need a bit of time for the auto trigger to start up so we have
+ // something to throw out. 1 khz trigger, so 2 ms gives us 2 cycles
+ // to hit it worst case.
+ std::this_thread::sleep_for(chrono::milliseconds(2));
+
+ // Throw out the first sample. It is almost always faulted due to
+ // how we start up, and it isn't worth tracking for downstream users
+ // to look at.
+ to_read_ = absl::MakeSpan(read_data_);
+ CHECK_EQ(spi_->ReadAutoReceivedData(
+ to_read_.data(), to_read_.size(),
+ 1000.0 /* block for up to 1 second */),
+ static_cast<int>(to_read_.size()))
+ << ": Failed to read first sample.";
success = true;
}
}
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 087c413..00896bb 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -441,6 +441,7 @@
":imu_batch_ts_fbs",
"//aos:configuration_ts_fbs",
"//aos/network/www:aos_plotter",
+ "//aos/network/www:plotter",
"//aos/network/www:reflection_ts",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
diff --git a/frc971/wpilib/imu_plot_utils.ts b/frc971/wpilib/imu_plot_utils.ts
index 8193c29..ee1a190 100644
--- a/frc971/wpilib/imu_plot_utils.ts
+++ b/frc971/wpilib/imu_plot_utils.ts
@@ -3,6 +3,7 @@
import * as configuration from 'org_frc971/aos/configuration_generated';
import * as imu from 'org_frc971/frc971/wpilib/imu_batch_generated';
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';
@@ -14,7 +15,7 @@
export class ImuMessageHandler extends MessageHandler {
// Calculated magnitude of the measured acceleration from the IMU.
- private acceleration_magnitudes: number[] = [];
+ private acceleration_magnitudes: Point[] = [];
constructor(private readonly schema: Schema) {
super(schema);
}
@@ -36,37 +37,38 @@
}
const time = message.monotonicTimestampNs().toFloat64() * 1e-9;
this.messages.push(new TimestampedMessage(table, time));
- this.acceleration_magnitudes.push(time);
- this.acceleration_magnitudes.push(Math.hypot(
- message.accelerometerX(), message.accelerometerY(),
- message.accelerometerZ()));
+ this.acceleration_magnitudes.push(new Point(
+ time,
+ Math.hypot(
+ message.accelerometerX(), message.accelerometerY(),
+ message.accelerometerZ())));
}
}
// Computes a moving average for a given input, using a basic window centered
// on each value.
- private movingAverageCentered(input: Float32Array): Float32Array {
- const num_measurements = input.length / 2;
- const filtered_measurements = new Float32Array(input);
+ private movingAverageCentered(input: Point[]): Point[] {
+ const num_measurements = input.length;
+ const filtered_measurements = [];
for (let ii = 0; ii < num_measurements; ++ii) {
let sum = 0;
let count = 0;
for (let jj = Math.max(0, Math.ceil(ii - FILTER_WINDOW_SIZE / 2));
jj < Math.min(num_measurements, ii + FILTER_WINDOW_SIZE / 2); ++jj) {
- sum += input[jj * 2 + 1];
+ sum += input[jj].y;
++count;
}
- filtered_measurements[ii * 2 + 1] = sum / count;
+ filtered_measurements.push(new Point(input[ii].x, sum / count));
}
- return new Float32Array(filtered_measurements);
+ return filtered_measurements;
}
- getField(field: string[]): Float32Array {
+ getField(field: string[]): Point[] {
// Any requested input that ends with "_filtered" will get a moving average
// applied to the original field.
const filtered_suffix = "_filtered";
if (field[0] == "acceleration_magnitude") {
- return new Float32Array(this.acceleration_magnitudes);
+ return this.acceleration_magnitudes;
} else if (field[0].endsWith(filtered_suffix)) {
return this.movingAverageCentered(this.getField(
[field[0].slice(0, field[0].length - filtered_suffix.length)]));
diff --git a/y2020/control_loops/superstructure/shooter/flywheel_controller.h b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
index 5fdac39..4eafe47 100644
--- a/y2020/control_loops/superstructure/shooter/flywheel_controller.h
+++ b/y2020/control_loops/superstructure/shooter/flywheel_controller.h
@@ -29,6 +29,7 @@
// Sets the velocity goal in radians/sec
void set_goal(double angular_velocity_goal);
+ double goal() const { return last_goal_; }
// Sets the current encoder position in radians
void set_position(double current_position,
const aos::monotonic_clock::time_point position_timestamp);
diff --git a/y2020/control_loops/superstructure/shooter/shooter.h b/y2020/control_loops/superstructure/shooter/shooter.h
index f72eeeb..1a46949 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.h
+++ b/y2020/control_loops/superstructure/shooter/shooter.h
@@ -26,6 +26,9 @@
bool ready() { return ready_; }
+ float finisher_goal() const { return finisher_.goal(); }
+ float accelerator_goal() const { return accelerator_left_.goal(); }
+
private:
FlywheelController finisher_, accelerator_left_, accelerator_right_;
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index 91e782e..9a48095 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -204,9 +204,8 @@
if (unsafe_goal) {
output_struct.washing_machine_spinner_voltage = 0.0;
if (unsafe_goal->shooting()) {
- if (shooter_.ready() &&
- unsafe_goal->shooter()->velocity_accelerator() > 10.0 &&
- unsafe_goal->shooter()->velocity_finisher() > 10.0) {
+ if (shooter_.ready() && shooter_.finisher_goal() > 10.0 &&
+ shooter_.accelerator_goal() > 10.0) {
output_struct.feeder_voltage = 12.0;
} else {
output_struct.feeder_voltage = 0.0;