Merge "Add install of matplotlib to raspberry pi disk image"
diff --git a/aos/network/www/BUILD b/aos/network/www/BUILD
index 1caa57c..5b951bd 100644
--- a/aos/network/www/BUILD
+++ b/aos/network/www/BUILD
@@ -93,12 +93,22 @@
)
ts_library(
+ name = "colors",
+ srcs = [
+ "colors.ts",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+ts_library(
name = "plotter",
srcs = [
"plotter.ts",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
+ deps = [":colors"],
)
ts_library(
diff --git a/aos/network/www/aos_plotter.ts b/aos/network/www/aos_plotter.ts
index a84fc66..35c27a7 100644
--- a/aos/network/www/aos_plotter.ts
+++ b/aos/network/www/aos_plotter.ts
@@ -60,6 +60,32 @@
this.messages.push(
new TimestampedMessage(Table.getRootTable(new ByteBuffer(data)), time));
}
+ private readField<T>(
+ message: Table, fieldName: string,
+ normalReader: (message: Table, name: string) => T | null,
+ vectorReader: (message: Table, name: string) => T[] | null): T[]|null {
+ // Typescript handles bindings in non-obvious ways that aren't caught well
+ // by the compiler.
+ normalReader = normalReader.bind(this.parser);
+ vectorReader = vectorReader.bind(this.parser);
+ const regex = /(.*)\[([0-9]*)\]/;
+ const match = fieldName.match(regex);
+ if (match) {
+ const name = match[1];
+ const vector = vectorReader(message, name);
+ if (vector === null) {
+ return null;
+ }
+ if (match[2] === "") {
+ return vector;
+ } else {
+ const index = parseInt(match[2]);
+ return (index < vector.length) ? [vector[index]] : null;
+ }
+ }
+ const singleResult = normalReader(message, fieldName);
+ return singleResult ? [singleResult] : null;
+ }
// Returns a time-series of every single instance of the given field. Format
// of the return value is [time0, value0, time1, value1,... timeN, valueN],
// to match with the Line.setPoint() interface.
@@ -70,41 +96,44 @@
getField(field: string[]): Float32Array {
const fieldName = field[field.length - 1];
const subMessage = field.slice(0, field.length - 1);
- const results = new Float32Array(this.messages.length * 2);
+ const results = [];
for (let ii = 0; ii < this.messages.length; ++ii) {
- let message = this.messages[ii].message;
+ let tables = [this.messages[ii].message];
for (const subMessageName of subMessage) {
- message = this.parser.readTable(message, subMessageName);
- if (message === undefined) {
- break;
+ let nextTables = [];
+ for (const table of tables) {
+ const nextTable = this.readField(
+ table, subMessageName, Parser.prototype.readTable,
+ Parser.prototype.readVectorOfTables);
+ if (nextTable === null) {
+ continue;
+ }
+ nextTables = nextTables.concat(nextTable);
}
+ tables = nextTables;
}
- results[ii * 2] = this.messages[ii].time;
- const regex = /(.*)\[([0-9]*)\]/;
- let name = fieldName;
- let match = fieldName.match(regex);
- let index = undefined;
- if (match) {
- name = match[1]
- index = parseInt(match[2])
- }
- if (message === undefined) {
- results[ii * 2 + 1] = NaN;
+ const time = this.messages[ii].time;
+ if (tables.length === 0) {
+ results.push(time);
+ results.push(NaN);
} else {
- if (index === undefined) {
- results[ii * 2 + 1] = this.parser.readScalar(message, name);
- } else {
- const vector =
- this.parser.readVectorOfScalars(message, name);
- if (index < vector.length) {
- results[ii * 2 + 1] = vector[index];
+ 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);
} else {
- results[ii * 2 + 1] = NaN;
+ for (const value of values) {
+ results.push(time);
+ results.push((value === null) ? NaN : value);
+ }
}
}
}
}
- return results;
+ return new Float32Array(results);
}
numMessages(): number {
return this.messages.length;
@@ -151,23 +180,14 @@
}
export class AosPlotter {
-
public static readonly TIME: string = "Monotonic Time (sec)";
public static readonly DEFAULT_WIDTH: number = 900;
public static readonly DEFAULT_HEIGHT: number = 400;
- public static readonly RED: number[] = [1, 0, 0];
- public static readonly GREEN: number[] = [0, 1, 0];
- public static readonly BLUE: number[] = [0, 0, 1];
- public static readonly BROWN: number[] = [0.6, 0.3, 0];
- public static readonly PINK: number[] = [1, 0.3, 0.6];
- public static readonly CYAN: number[] = [0.3, 1, 1];
- public static readonly WHITE: number[] = [1, 1, 1];
-
-
private plots: AosPlot[] = [];
private messages = new Set<MessageHandler>();
+ private lowestHeight = 0;
constructor(private readonly connection: Connection) {
// Set up to redraw at some regular interval. The exact rate is unimportant.
setInterval(() => {
@@ -199,7 +219,13 @@
}
// Add a new figure at the provided position with the provided size within
// parentElement.
- addPlot(parentElement: Element, topLeft: number[], size: number[]): AosPlot {
+ addPlot(
+ parentElement: Element, topLeft: number[]|null = null,
+ size: number[] = [AosPlotter.DEFAULT_WIDTH, AosPlotter.DEFAULT_HEIGHT]):
+ AosPlot {
+ if (topLeft === null) {
+ topLeft = [0, this.lowestHeight];
+ }
const div = document.createElement("div");
div.style.top = topLeft[1].toString();
div.style.left = topLeft[0].toString();
@@ -210,6 +236,8 @@
newPlot.linkXAxis(plot.plot);
}
this.plots.push(new AosPlot(this, newPlot));
+ // Height goes up as you go down.
+ this.lowestHeight = Math.max(topLeft[1] + size[1], this.lowestHeight);
return this.plots[this.plots.length - 1];
}
private draw(): void {
diff --git a/aos/network/www/colors.ts b/aos/network/www/colors.ts
new file mode 100644
index 0000000..1a1c0b3
--- /dev/null
+++ b/aos/network/www/colors.ts
@@ -0,0 +1,7 @@
+export const RED: number[] = [1, 0, 0];
+export const GREEN: number[] = [0, 1, 0];
+export const BLUE: number[] = [0, 0, 1];
+export const BROWN: number[] = [0.6, 0.3, 0];
+export const PINK: number[] = [1, 0.3, 0.6];
+export const CYAN: number[] = [0.3, 1, 1];
+export const WHITE: number[] = [1, 1, 1];
diff --git a/aos/network/www/demo_plot.ts b/aos/network/www/demo_plot.ts
index d223757..ebdff7c 100644
--- a/aos/network/www/demo_plot.ts
+++ b/aos/network/www/demo_plot.ts
@@ -19,11 +19,11 @@
import Connection = proxy.Connection;
export function plotDemo(conn: Connection, parentDiv: Element): void {
- const width = 900;
- const height = 400;
+ const width = AosPlotter.DEFAULT_WIDTH;
+ const height = AosPlotter.DEFAULT_HEIGHT;
const benchmarkDiv = document.createElement('div');
- benchmarkDiv.style.top = height.toString();
+ benchmarkDiv.style.top = (height * 2).toString();
benchmarkDiv.style.left = '0';
benchmarkDiv.style.position = 'absolute';
parentDiv.appendChild(benchmarkDiv);
@@ -33,20 +33,45 @@
const aosPlotter = new AosPlotter(conn);
{
- // Setup a plot that just shows the PID of each timing report message.
- // For the basic live_web_plotter_demo, this will be a boring line showing
- // just the PID of the proxy process. On a real system, or against a logfile,
- // this would show the PIDs of all active processes.
+ // Setup a plot that shows some arbitrary PDP current values.
+ const pdpValues =
+ aosPlotter.addMessageSource('/aos', 'frc971.PDPValues');
+ const timingPlot = aosPlotter.addPlot(parentDiv);
+ timingPlot.plot.getAxisLabels().setTitle('Current Values');
+ timingPlot.plot.getAxisLabels().setYLabel('Current (Amps)');
+ timingPlot.plot.getAxisLabels().setXLabel('Monotonic Send Time (sec)');
+ // Displays points for every single current sample at each time-point.
+ const allValuesLine = timingPlot.addMessageLine(pdpValues, ['currents[]']);
+ allValuesLine.setDrawLine(false);
+ allValuesLine.setPointSize(5);
+ // Displays a line for the current along channel 1.
+ const singleValueLine = timingPlot.addMessageLine(pdpValues, ['currents[1]']);
+ singleValueLine.setDrawLine(true);
+ singleValueLine.setPointSize(0);
+ const voltageLine = timingPlot.addMessageLine(pdpValues, ['voltage']);
+ voltageLine.setPointSize(0);
+ }
+
+ {
const timingReport =
aosPlotter.addMessageSource('/aos', 'aos.timing.Report');
- const timingPlot =
- aosPlotter.addPlot(parentDiv, [0, 0], [width, height]);
- timingPlot.plot.getAxisLabels().setTitle('Timing Report PID');
+ // Setup a plot that just shows some arbitrary timing data.
+ const timingPlot = aosPlotter.addPlot(parentDiv);
+ timingPlot.plot.getAxisLabels().setTitle('Timing Report Wakeups');
timingPlot.plot.getAxisLabels().setYLabel('PID');
timingPlot.plot.getAxisLabels().setXLabel('Monotonic Send Time (sec)');
- const msgLine = timingPlot.addMessageLine(timingReport, ['pid']);
- msgLine.setDrawLine(false);
- msgLine.setPointSize(5);
+ // Show *all* the wakeup latencies for all timers.
+ const allValuesLine = timingPlot.addMessageLine(
+ timingReport, ['timers[]', 'wakeup_latency', 'average']);
+ allValuesLine.setDrawLine(false);
+ allValuesLine.setPointSize(5);
+ // Show *all* the wakeup latencies for the first timer in each timing report
+ // (this is not actually all that helpful unless you were to also filter by
+ // PID).
+ const singleValueLine = timingPlot.addMessageLine(
+ timingReport, ['timers[0]', 'wakeup_latency', 'average']);
+ singleValueLine.setDrawLine(true);
+ singleValueLine.setPointSize(0);
}
// Set up and draw the benchmarking plot.
diff --git a/aos/network/www/plotter.ts b/aos/network/www/plotter.ts
index 4c73a65..21d9834 100644
--- a/aos/network/www/plotter.ts
+++ b/aos/network/www/plotter.ts
@@ -1,3 +1,4 @@
+import * as Colors from 'org_frc971/aos/network/www/colors';
// Multiplies all the values in the provided array by scale.
function scaleVec(vec: number[], scale: number): number[] {
const scaled: number[] = [];
@@ -72,9 +73,10 @@
return this._color;
}
- setColor(newColor: number[]) {
+ setColor(newColor: number[]): Line {
this._color = newColor;
this._hasUpdate = true;
+ return this;
}
// Get/set the size of the markers to draw, in pixels (zero means no markers).
@@ -82,9 +84,10 @@
return this._pointSize;
}
- setPointSize(size: number) {
+ setPointSize(size: number): Line {
this._pointSize = size;
this._hasUpdate = true;
+ return this;
}
// Get/set whether we draw a line between the points (i.e., setting this to
@@ -94,9 +97,10 @@
return this._drawLine;
}
- setDrawLine(newDrawLine: boolean) {
+ setDrawLine(newDrawLine: boolean): Line {
this._drawLine = newDrawLine;
this._hasUpdate = true;
+ return this;
}
// Set the points to render. The points in the line are ordered and should
@@ -135,8 +139,9 @@
}
// Get/set the label to use for the line when drawing the legend.
- setLabel(label: string) {
+ setLabel(label: string): Line {
this._label = label;
+ return this;
}
label(): string|null {
@@ -344,6 +349,12 @@
private xGridLines: Line[] = [];
private yGridLines: Line[] = [];
+ public static readonly COLOR_CYCLE = [
+ Colors.RED, Colors.GREEN, Colors.BLUE, Colors.BROWN, Colors.PINK,
+ Colors.CYAN, Colors.WHITE
+ ];
+ private colorCycleIndex = 0;
+
constructor(public readonly ctx: WebGLRenderingContext) {
this.program = this.compileShaders();
this.scaleLocation = this.ctx.getUniformLocation(this.program, 'scale');
@@ -509,9 +520,13 @@
return program;
}
- addLine(): Line {
+ addLine(useColorCycle: boolean = true): Line {
this.lines.push(new Line(this.ctx, this.program, this.vertexBuffer));
- return this.lines[this.lines.length - 1];
+ const line = this.lines[this.lines.length - 1];
+ if (useColorCycle) {
+ line.setColor(LineDrawer.COLOR_CYCLE[this.colorCycleIndex++]);
+ }
+ return line;
}
minValues(): number[] {
@@ -812,8 +827,8 @@
new AxisLabels(textCtx, this.drawer, this.axisLabelBuffer);
this.legend = new Legend(textCtx, this.drawer.getLines());
- this.zoomRectangle = this.getDrawer().addLine();
- this.zoomRectangle.setColor([1, 1, 1]);
+ this.zoomRectangle = this.getDrawer().addLine(false);
+ this.zoomRectangle.setColor(Colors.WHITE);
this.zoomRectangle.setPointSize(0);
this.draw();
diff --git a/aos/starter/BUILD b/aos/starter/BUILD
index fbd29fd..683d23d 100644
--- a/aos/starter/BUILD
+++ b/aos/starter/BUILD
@@ -95,6 +95,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
":starter_rpc_lib",
+ "//aos/time:time",
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings:str_format",
],
diff --git a/aos/starter/starter_cmd.cc b/aos/starter/starter_cmd.cc
index 1381fa3..8f9e125 100644
--- a/aos/starter/starter_cmd.cc
+++ b/aos/starter/starter_cmd.cc
@@ -1,11 +1,13 @@
#include <chrono>
#include <functional>
#include <iostream>
+#include <optional>
#include <unordered_map>
#include "absl/strings/str_format.h"
#include "aos/init.h"
#include "aos/json_to_flatbuffer.h"
+#include "aos/time/time.h"
#include "gflags/gflags.h"
#include "starter_rpc_lib.h"
@@ -13,25 +15,49 @@
namespace {
+namespace chrono = std::chrono;
+
static const std::unordered_map<std::string, aos::starter::Command>
kCommandConversions{{"start", aos::starter::Command::START},
{"stop", aos::starter::Command::STOP},
{"restart", aos::starter::Command::RESTART}};
+void PrintKey() {
+ absl::PrintF("%-30s %-30s %s\n\n", "Name", "Time since last started", "State");
+}
+
+void PrintApplicationStatus(const aos::starter::ApplicationStatus *app_status,
+ const aos::monotonic_clock::time_point &time) {
+ const auto last_start_time =
+ aos::monotonic_clock::time_point(chrono::nanoseconds(app_status->last_start_time()));
+ const auto time_running =
+ chrono::duration_cast<chrono::seconds>(time - last_start_time);
+ absl::PrintF("%-30s %-30s %s\n", app_status->name()->string_view(),
+ std::to_string(time_running.count()) + 's',
+ aos::starter::EnumNameState(app_status->state()));
+}
+
bool GetStarterStatus(int argc, char **argv, const aos::Configuration *config) {
if (argc == 1) {
// Print status for all processes.
- auto status = aos::starter::GetStarterStatus(config);
- for (const aos::starter::ApplicationStatus *app_status :
- *status.message().statuses()) {
- absl::PrintF("%-30s %s\n", app_status->name()->string_view(),
- aos::starter::EnumNameState(app_status->state()));
+ const auto optional_status = aos::starter::GetStarterStatus(config);
+ if (optional_status) {
+ auto status = *optional_status;
+ const auto time = aos::monotonic_clock::now();
+ PrintKey();
+ for (const aos::starter::ApplicationStatus *app_status :
+ *status.message().statuses()) {
+ PrintApplicationStatus(app_status, time);
+ }
+ } else {
+ LOG(WARNING) << "No status found";
}
} else if (argc == 2) {
// Print status for the specified process.
const char *application_name = argv[1];
auto status = aos::starter::GetStatus(application_name, config);
- std::cout << aos::FlatbufferToJson(&status.message()) << '\n';
+ PrintKey();
+ PrintApplicationStatus(&status.message(), aos::monotonic_clock::now());
} else {
LOG(ERROR) << "The \"status\" command requires zero or one arguments.";
return true;
@@ -58,7 +84,7 @@
const char *application_name = argv[1];
if (aos::starter::SendCommandBlocking(command, application_name, config,
- std::chrono::seconds(3))) {
+ chrono::seconds(3))) {
switch (command) {
case aos::starter::Command::START:
std::cout << "Successfully started " << application_name << '\n';
diff --git a/aos/starter/starter_rpc_lib.cc b/aos/starter/starter_rpc_lib.cc
index 89fc2c2..20f6f5a 100644
--- a/aos/starter/starter_rpc_lib.cc
+++ b/aos/starter/starter_rpc_lib.cc
@@ -127,18 +127,15 @@
aos::starter::ApplicationStatus>::Empty();
}
-const aos::FlatbufferVector<aos::starter::Status> GetStarterStatus(
+std::optional<const aos::FlatbufferVector<aos::starter::Status>> GetStarterStatus(
const aos::Configuration *config) {
ShmEventLoop event_loop(config);
event_loop.SkipAosLog();
auto status_fetcher = event_loop.MakeFetcher<aos::starter::Status>("/aos");
status_fetcher.Fetch();
- if (status_fetcher) {
- return status_fetcher.CopyFlatBuffer();
- } else {
- return FlatbufferVector<aos::starter::Status>::Empty();
- }
+ return (status_fetcher ? std::make_optional(status_fetcher.CopyFlatBuffer()) :
+ std::nullopt);
}
} // namespace starter
diff --git a/aos/starter/starter_rpc_lib.h b/aos/starter/starter_rpc_lib.h
index 152016a..24c757e 100644
--- a/aos/starter/starter_rpc_lib.h
+++ b/aos/starter/starter_rpc_lib.h
@@ -2,6 +2,7 @@
#define AOS_STARTER_STARTER_RPC_LIB_H_
#include <chrono>
+#include <optional>
#include "aos/configuration.h"
#include "aos/starter/starter_generated.h"
@@ -32,7 +33,7 @@
// Fetches the entire status message of starter. Creates a temporary event loop
// from the provided config for fetching.
-const aos::FlatbufferVector<aos::starter::Status> GetStarterStatus(
+std::optional<const aos::FlatbufferVector<aos::starter::Status>> GetStarterStatus(
const aos::Configuration *config);
} // namespace starter
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index b72634a..ea83668 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -79,9 +79,13 @@
"//aos:configuration_ts_fbs",
"//aos/network/www:demo_plot",
"//aos/network/www:proxy",
+ "//frc971/control_loops/drivetrain:down_estimator_plotter",
"//frc971/control_loops/drivetrain:drivetrain_plotter",
"//frc971/control_loops/drivetrain:robot_state_plotter",
"//frc971/wpilib:imu_plotter",
+ "//y2020/control_loops/superstructure:accelerator_plotter",
+ "//y2020/control_loops/superstructure:finisher_plotter",
+ "//y2020/control_loops/superstructure:hood_plotter",
],
)
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index 780f944..4bf6c5a 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -24,8 +24,15 @@
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 {plotDownEstimator} from 'org_frc971/frc971/control_loops/drivetrain/down_estimator_plotter';
import {plotRobotState} from
'org_frc971/frc971/control_loops/drivetrain/robot_state_plotter'
+import {plotFinisher} from
+ 'org_frc971/y2020/control_loops/superstructure/finisher_plotter'
+import {plotAccelerator} from
+ 'org_frc971/y2020/control_loops/superstructure/accelerator_plotter'
+import {plotHood} from
+ 'org_frc971/y2020/control_loops/superstructure/hood_plotter'
import {plotDemo} from 'org_frc971/aos/network/www/demo_plot';
import {plotData} from 'org_frc971/frc971/analysis/plot_data_utils';
@@ -83,7 +90,11 @@
['Demo', new PlotState(plotDiv, plotDemo)],
['IMU', new PlotState(plotDiv, plotImu)],
['Drivetrain', new PlotState(plotDiv, plotDrivetrain)],
+ ['Down Estimator', new PlotState(plotDiv, plotDownEstimator)],
['Robot State', new PlotState(plotDiv, plotRobotState)],
+ ['Finisher', new PlotState(plotDiv, plotFinisher)],
+ ['Accelerator', new PlotState(plotDiv, plotAccelerator)],
+ ['Hood', new PlotState(plotDiv, plotHood)],
['C++ Plotter', new PlotState(plotDiv, plotData)],
]);
@@ -103,6 +114,8 @@
const conn = new Connection();
+let reloadOnChange = false;
+
conn.connect();
conn.addConfigHandler((config: Configuration) => {
@@ -122,6 +135,10 @@
// Set the URL so that if you reload you get back to this plot.
window.history.replaceState(
null, null, '?plot=' + encodeURIComponent(plotSelect.value));
+ if (reloadOnChange) {
+ window.location.reload();
+ }
+ reloadOnChange = true;
});
plotSelect.value = getDefaultPlot();
// Force the event to occur once at the start.
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 8c3274d..1da9db3 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -726,11 +726,24 @@
)
ts_library(
+ name = "down_estimator_plotter",
+ srcs = ["down_estimator_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ "//frc971/wpilib:imu_plot_utils",
+ ],
+)
+
+ts_library(
name = "drivetrain_plotter",
srcs = ["drivetrain_plotter.ts"],
target_compatible_with = ["@platforms//os:linux"],
deps = [
"//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
"//aos/network/www:proxy",
"//frc971/wpilib:imu_plot_utils",
],
@@ -742,6 +755,7 @@
target_compatible_with = ["@platforms//os:linux"],
deps = [
"//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
"//aos/network/www:proxy",
],
)
diff --git a/frc971/control_loops/drivetrain/down_estimator_plotter.ts b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
new file mode 100644
index 0000000..c6e414c
--- /dev/null
+++ b/frc971/control_loops/drivetrain/down_estimator_plotter.ts
@@ -0,0 +1,160 @@
+// Provides a basic plot for debugging IMU-related issues on a robot.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+export function plotDownEstimator(conn: Connection, element: Element): void {
+ const width = 900;
+ const height = 400;
+ const aosPlotter = new AosPlotter(conn);
+
+ const status = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+
+ const imu = aosPlotter.addRawMessageSource(
+ '/drivetrain', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ const accelPlot = aosPlotter.addPlot(element, [0, 0], [width, height]);
+ accelPlot.plot.getAxisLabels().setTitle(
+ 'Estimated Accelerations (x = forward, y = lateral, z = vertical)');
+ accelPlot.plot.getAxisLabels().setYLabel('Acceleration (m/s/s)');
+ accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const accelX = accelPlot.addMessageLine(status, ['down_estimator', 'accel_x']);
+ accelX.setColor(RED);
+ const accelY = accelPlot.addMessageLine(status, ['down_estimator', 'accel_y']);
+ accelY.setColor(GREEN);
+ const accelZ = accelPlot.addMessageLine(status, ['down_estimator', 'accel_z']);
+ accelZ.setColor(BLUE);
+
+ const velPlot = aosPlotter.addPlot(element, [0, height], [width, height]);
+ velPlot.plot.getAxisLabels().setTitle('Raw IMU Integrated Velocity');
+ velPlot.plot.getAxisLabels().setYLabel('Velocity (m/s)');
+ velPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const velX = velPlot.addMessageLine(status, ['down_estimator', 'velocity_x']);
+ velX.setColor(RED);
+ const velY = velPlot.addMessageLine(status, ['down_estimator', 'velocity_y']);
+ velY.setColor(GREEN);
+ const velZ = velPlot.addMessageLine(status, ['down_estimator', 'velocity_z']);
+ velZ.setColor(BLUE);
+
+ const gravityPlot = aosPlotter.addPlot(element, [0, height * 2], [width, height]);
+ gravityPlot.plot.getAxisLabels().setTitle('Accelerometer Magnitudes');
+ gravityPlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
+ gravityPlot.plot.setDefaultYRange([0.95, 1.05]);
+ const gravityLine =
+ gravityPlot.addMessageLine(status, ['down_estimator', 'gravity_magnitude']);
+ gravityLine.setColor(RED);
+ gravityLine.setDrawLine(false);
+ const accelMagnitudeLine =
+ gravityPlot.addMessageLine(imu, ['acceleration_magnitude_filtered']);
+ accelMagnitudeLine.setColor(BLUE);
+ accelMagnitudeLine.setLabel('Filtered Accel Magnitude (0.1 sec)');
+ accelMagnitudeLine.setDrawLine(false);
+
+ const orientationPlot =
+ aosPlotter.addPlot(element, [0, height * 3], [width, height]);
+ orientationPlot.plot.getAxisLabels().setTitle('Orientation');
+ orientationPlot.plot.getAxisLabels().setYLabel('Angle (rad)');
+
+ const roll = orientationPlot.addMessageLine(
+ status, ['down_estimator', 'lateral_pitch']);
+ roll.setColor(RED);
+ roll.setLabel('roll');
+ const pitch = orientationPlot.addMessageLine(
+ status, ['down_estimator', 'longitudinal_pitch']);
+ pitch.setColor(GREEN);
+ pitch.setLabel('pitch');
+ const yaw = orientationPlot.addMessageLine(
+ status, ['down_estimator', 'yaw']);
+ yaw.setColor(BLUE);
+ yaw.setLabel('yaw');
+
+ const imuAccelPlot = aosPlotter.addPlot(element, [0, height * 4], [width, height]);
+ imuAccelPlot.plot.getAxisLabels().setTitle('Filtered Accelerometer Readings');
+ imuAccelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
+ imuAccelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const imuAccelX = imuAccelPlot.addMessageLine(imu, ['accelerometer_x']);
+ imuAccelX.setColor(RED);
+ imuAccelX.setDrawLine(false);
+ const imuAccelY = imuAccelPlot.addMessageLine(imu, ['accelerometer_y']);
+ imuAccelY.setColor(GREEN);
+ imuAccelY.setDrawLine(false);
+ const imuAccelZ = imuAccelPlot.addMessageLine(imu, ['accelerometer_z']);
+ imuAccelZ.setColor(BLUE);
+ imuAccelZ.setDrawLine(false);
+
+ const imuAccelXFiltered = imuAccelPlot.addMessageLine(imu, ['accelerometer_x_filtered']);
+ imuAccelXFiltered.setColor(RED);
+ imuAccelXFiltered.setPointSize(0);
+ const imuAccelYFiltered = imuAccelPlot.addMessageLine(imu, ['accelerometer_y_filtered']);
+ imuAccelYFiltered.setColor(GREEN);
+ imuAccelYFiltered.setPointSize(0);
+ const imuAccelZFiltered = imuAccelPlot.addMessageLine(imu, ['accelerometer_z_filtered']);
+ imuAccelZFiltered.setColor(BLUE);
+ imuAccelZFiltered.setPointSize(0);
+
+ const expectedAccelX = imuAccelPlot.addMessageLine(
+ status, ['down_estimator', 'expected_accel_x']);
+ expectedAccelX.setColor(RED);
+ expectedAccelX.setPointSize(0);
+ const expectedAccelY = imuAccelPlot.addMessageLine(
+ status, ['down_estimator', 'expected_accel_y']);
+ expectedAccelY.setColor(GREEN);
+ expectedAccelY.setPointSize(0);
+ const expectedAccelZ = imuAccelPlot.addMessageLine(
+ status, ['down_estimator', 'expected_accel_z']);
+ expectedAccelZ.setColor(BLUE);
+ expectedAccelZ.setPointSize(0);
+
+ const gyroPlot = aosPlotter.addPlot(element, [0, height * 5], [width, 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(RED);
+ gyroZeroX.setPointSize(0);
+ gyroZeroX.setLabel('Gyro X Zero');
+ const gyroZeroY =
+ gyroPlot.addMessageLine(status, ['zeroing', 'gyro_y_average']);
+ gyroZeroY.setColor(GREEN);
+ gyroZeroY.setPointSize(0);
+ gyroZeroY.setLabel('Gyro Y Zero');
+ const gyroZeroZ =
+ gyroPlot.addMessageLine(status, ['zeroing', 'gyro_z_average']);
+ gyroZeroZ.setColor(BLUE);
+ gyroZeroZ.setPointSize(0);
+ gyroZeroZ.setLabel('Gyro Z Zero');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor(RED);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor(GREEN);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor(BLUE);
+
+ const statePlot = aosPlotter.addPlot(element, [0, height * 6], [width, height / 2]);
+ statePlot.plot.getAxisLabels().setTitle('IMU State');
+ statePlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
+
+ const zeroedLine =
+ statePlot.addMessageLine(status, ['zeroing', 'zeroed']);
+ zeroedLine.setColor(RED);
+ zeroedLine.setDrawLine(false);
+ const consecutiveStill =
+ statePlot.addMessageLine(status, ['down_estimator', 'consecutive_still']);
+ consecutiveStill.setColor(BLUE);
+ consecutiveStill.setPointSize(0);
+ const faultedLine =
+ statePlot.addMessageLine(status, ['zeroing', 'faulted']);
+ faultedLine.setColor(GREEN);
+ faultedLine.setPointSize(0);
+}
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index 0c01f6a..f55f965 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -2,19 +2,13 @@
import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
import * as proxy from 'org_frc971/aos/network/www/proxy';
+import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
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 plotDrivetrain(conn: Connection, element: Element): void {
const aosPlotter = new AosPlotter(conn);
diff --git a/frc971/control_loops/drivetrain/robot_state_plotter.ts b/frc971/control_loops/drivetrain/robot_state_plotter.ts
index f42c50b..829df25 100644
--- a/frc971/control_loops/drivetrain/robot_state_plotter.ts
+++ b/frc971/control_loops/drivetrain/robot_state_plotter.ts
@@ -1,19 +1,13 @@
// 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 {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
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);
diff --git a/frc971/wpilib/imu_plot_utils.ts b/frc971/wpilib/imu_plot_utils.ts
index 4e6c371..8193c29 100644
--- a/frc971/wpilib/imu_plot_utils.ts
+++ b/frc971/wpilib/imu_plot_utils.ts
@@ -10,10 +10,17 @@
import IMUValuesBatch = imu.frc971.IMUValuesBatch;
import IMUValues = imu.frc971.IMUValues;
+const FILTER_WINDOW_SIZE = 100;
+
export class ImuMessageHandler extends MessageHandler {
+ // Calculated magnitude of the measured acceleration from the IMU.
+ private acceleration_magnitudes: number[] = [];
constructor(private readonly schema: Schema) {
super(schema);
}
+ private readScalar(table: Table, fieldName: string): number {
+ return this.parser.readScalar(table, fieldName);
+ }
addMessage(data: Uint8Array, time: number): void {
const batch = IMUValuesBatch.getRootAsIMUValuesBatch(
new ByteBuffer(data) as unknown as flatbuffers.ByteBuffer);
@@ -27,8 +34,44 @@
console.log(this.parser.toObject(table));
continue;
}
- this.messages.push(new TimestampedMessage(
- table, message.monotonicTimestampNs().toFloat64() * 1e-9));
+ 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()));
+ }
+ }
+
+ // 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);
+ 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];
+ ++count;
+ }
+ filtered_measurements[ii * 2 + 1] = sum / count;
+ }
+ return new Float32Array(filtered_measurements);
+ }
+
+ getField(field: string[]): Float32Array {
+ // 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);
+ } else if (field[0].endsWith(filtered_suffix)) {
+ return this.movingAverageCentered(this.getField(
+ [field[0].slice(0, field[0].length - filtered_suffix.length)]));
+ } else {
+ return super.getField(field);
}
}
}
diff --git a/y2020/constants.cc b/y2020/constants.cc
index 0e36f0f..b3551aa 100644
--- a/y2020/constants.cc
+++ b/y2020/constants.cc
@@ -35,7 +35,7 @@
*const hood = &r->hood;
// Hood constants.
- hood->zeroing_voltage = 3.0;
+ hood->zeroing_voltage = 2.0;
hood->operating_voltage = 12.0;
hood->zeroing_profile_params = {0.5, 3.0};
hood->default_profile_params = {6.0, 30.0};
@@ -109,8 +109,6 @@
break;
case kCompTeamNumber:
- hood->zeroing_constants.measured_absolute_position = 0.266691815725476;
-
intake->zeroing_constants.measured_absolute_position =
1.42977866919024 - Values::kIntakeZero();
@@ -119,17 +117,18 @@
turret_params->zeroing_constants.measured_absolute_position =
0.547478339799516;
- hood->zeroing_constants.measured_absolute_position = 0.03207;
- hood->zeroing_constants.single_turn_measured_absolute_position = 0.31055;
+ hood->zeroing_constants.measured_absolute_position = 0.0344482433884915;
+ hood->zeroing_constants.single_turn_measured_absolute_position =
+ 0.31055891442198;
break;
case kPracticeTeamNumber:
hood->zeroing_constants.measured_absolute_position = 0.0;
- intake->zeroing_constants.measured_absolute_position = 0.0;
+ intake->zeroing_constants.measured_absolute_position = 0.347;
- turret->potentiometer_offset = 0.0;
- turret_params->zeroing_constants.measured_absolute_position = 0.0;
+ turret->potentiometer_offset = 5.3931926228241;
+ turret_params->zeroing_constants.measured_absolute_position = 4.22;
break;
case Values::kCodingRobotTeamNumber:
diff --git a/y2020/constants.h b/y2020/constants.h
index e55a70b..de3303b 100644
--- a/y2020/constants.h
+++ b/y2020/constants.h
@@ -61,10 +61,10 @@
static constexpr ::frc971::constants::Range kHoodRange() {
return ::frc971::constants::Range{
- 0.00, // Back Hard
- 0.64, // Front Hard
- 0.01, // Back Soft
- 0.63 // Front Soft
+ -0.01, // Back Hard
+ 0.65, // Front Hard
+ 0.00, // Back Soft
+ 0.63 // Front Soft
};
}
@@ -156,7 +156,7 @@
static constexpr double kFinisherEncoderCountsPerRevolution() {
return 2048.0;
}
- static constexpr double kFinisherEncoderRatio() { return 30.0 / 40.0; }
+ static constexpr double kFinisherEncoderRatio() { return 36.0 / 40.0; }
static constexpr double kMaxFinisherEncoderPulsesPerSecond() {
return control_loops::superstructure::finisher::kFreeSpeed / (2.0 * M_PI) *
diff --git a/y2020/control_loops/python/finisher.py b/y2020/control_loops/python/finisher.py
index 6da1c03..035586a 100644
--- a/y2020/control_loops/python/finisher.py
+++ b/y2020/control_loops/python/finisher.py
@@ -13,27 +13,25 @@
gflags.DEFINE_bool('plot', False, 'If true, plot the loop response.')
-# Inertia for a single 4" diameter, 2" wide neopreme wheel.
-J_wheel = 0.000319 * 2.0 * 6.0
# Gear ratio to the final wheel.
# 40 tooth on the flywheel
# 48 for the falcon.
# 60 tooth on the outer wheel.
G = 48.0 / 40.0
# Overall flywheel inertia.
-J = J_wheel * (1.0 + (40.0 / 60.0)**2.0)
+J = 0.00507464
# The position and velocity are measured for the final wheel.
kFinisher = flywheel.FlywheelParams(
name='Finisher',
- motor=control_loop.Falcon(),
+ motor=control_loop.NMotor(control_loop.Falcon(), 2),
G=G,
J=J,
q_pos=0.01,
q_vel=100.0,
q_voltage=6.0,
r_pos=0.05,
- controller_poles=[.92])
+ controller_poles=[.90])
def main(argv):
diff --git a/y2020/control_loops/python/hood.py b/y2020/control_loops/python/hood.py
index db6b727..eacb7fd 100644
--- a/y2020/control_loops/python/hood.py
+++ b/y2020/control_loops/python/hood.py
@@ -21,9 +21,9 @@
# joint. We are currently electing to ignore potential non-linearity.
range_of_travel_radians = (38.0 * numpy.pi / 180.0)
-# 0.472441 inches/turn (12 mm)
+# 0.5 inches/turn
# 6.7725 inches of travel
-turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.472441
+turns_of_leadscrew_per_range_of_travel = 6.7725 / 0.5
radians_per_turn = range_of_travel_radians / turns_of_leadscrew_per_range_of_travel
@@ -33,13 +33,13 @@
name='Hood',
motor=control_loop.BAG(),
G=radians_per_turn_of_motor / (2.0 * numpy.pi),
- J=4.0,
+ J=0.1,
q_pos=0.15,
- q_vel=5.0,
+ q_vel=10.0,
kalman_q_pos=0.12,
kalman_q_vel=10.0,
- kalman_q_voltage=15.0,
- kalman_r_position=0.05)
+ kalman_q_voltage=30.0,
+ kalman_r_position=0.02)
def main(argv):
diff --git a/y2020/control_loops/superstructure/BUILD b/y2020/control_loops/superstructure/BUILD
index 3ea3dbf..fbf6a57 100644
--- a/y2020/control_loops/superstructure/BUILD
+++ b/y2020/control_loops/superstructure/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ["//visibility:public"])
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
flatbuffer_cc_library(
name = "superstructure_goal_fbs",
@@ -135,3 +136,36 @@
"//frc971/control_loops:profiled_subsystem_fbs",
],
)
+
+ts_library(
+ name = "finisher_plotter",
+ srcs = ["finisher_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 = "accelerator_plotter",
+ srcs = ["accelerator_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 = "hood_plotter",
+ srcs = ["hood_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:colors",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/y2020/control_loops/superstructure/accelerator_plotter.ts b/y2020/control_loops/superstructure/accelerator_plotter.ts
new file mode 100644
index 0000000..eaca1b8
--- /dev/null
+++ b/y2020/control_loops/superstructure/accelerator_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 {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotAccelerator(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 pdpValues = aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const velocityPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
+ velocityPlot.plot.setDefaultYRange([0.0, 450.0]);
+
+ velocityPlot.addMessageLine(goal, ['shooter', 'velocity_accelerator']).setColor(BLUE).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'avg_angular_velocity']).setColor(RED).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'avg_angular_velocity']).setColor(PINK).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'angular_velocity']).setColor(GREEN).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'angular_velocity']).setColor(CYAN).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'dt_angular_velocity']).setColor(BLUE).setPointSize(0.0);
+
+ const voltagePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Volts');
+ voltagePlot.plot.setDefaultYRange([-4.0, 16.0]);
+
+ voltagePlot.addMessageLine(output, ['accelerator_left_voltage']).setColor(BLUE).setPointSize(0.0);
+ voltagePlot.addMessageLine(output, ['accelerator_right_voltage']).setColor(BROWN).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['shooter', 'accelerator_left', 'voltage_error']).setColor(RED).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['shooter', 'accelerator_right', 'voltage_error']).setColor(PINK).setPointSize(0.0);
+ voltagePlot.addMessageLine(robotState, ['voltage_battery']).setColor(GREEN).setPointSize(0.0);
+
+
+ const currentPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ currentPlot.plot.getAxisLabels().setTitle('Current');
+ currentPlot.plot.getAxisLabels().setXLabel(TIME);
+ currentPlot.plot.getAxisLabels().setYLabel('Amps');
+ currentPlot.plot.setDefaultYRange([0.0, 80.0]);
+
+ currentPlot.addMessageLine(pdpValues, ['currents[14]']).setColor(GREEN).setPointSize(0.0);
+ currentPlot.addMessageLine(pdpValues, ['currents[15]']).setColor(BLUE).setPointSize(0.0);
+ currentPlot.addMessageLine(status, ['shooter', 'accelerator_left', 'commanded_current']).setColor(RED).setPointSize(0.0);
+ currentPlot.addMessageLine(status, ['shooter', 'accelerator_right', 'commanded_current']).setColor(PINK).setPointSize(0.0);
+}
diff --git a/y2020/control_loops/superstructure/finisher_plotter.ts b/y2020/control_loops/superstructure/finisher_plotter.ts
new file mode 100644
index 0000000..18052cc
--- /dev/null
+++ b/y2020/control_loops/superstructure/finisher_plotter.ts
@@ -0,0 +1,60 @@
+// 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 {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotFinisher(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 pdpValues = aosPlotter.addMessageSource('/roborio/aos', 'frc971.PDPValues');
+ const robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const velocityPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ velocityPlot.plot.getAxisLabels().setTitle('Velocity');
+ velocityPlot.plot.getAxisLabels().setXLabel(TIME);
+ velocityPlot.plot.getAxisLabels().setYLabel('rad/s');
+ velocityPlot.plot.setDefaultYRange([0.0, 450.0]);
+
+ velocityPlot.addMessageLine(goal, ['shooter', 'velocity_finisher']).setColor(BLUE).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'avg_angular_velocity']).setColor(RED).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'angular_velocity']).setColor(GREEN).setPointSize(0.0);
+ velocityPlot.addMessageLine(status, ['shooter', 'finisher', 'dt_angular_velocity']).setColor(PINK).setPointSize(0.0);
+
+ const voltagePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Volts');
+ voltagePlot.plot.setDefaultYRange([-4.0, 16.0]);
+
+ voltagePlot.addMessageLine(output, ['finisher_voltage']).setColor(BLUE).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['shooter', 'finisher', 'voltage_error']).setColor(RED).setPointSize(0.0);
+ voltagePlot.addMessageLine(robotState, ['voltage_battery']).setColor(GREEN).setPointSize(0.0);
+
+
+ const currentPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ currentPlot.plot.getAxisLabels().setTitle('Current');
+ currentPlot.plot.getAxisLabels().setXLabel(TIME);
+ currentPlot.plot.getAxisLabels().setYLabel('Amps');
+ currentPlot.plot.setDefaultYRange([0.0, 80.0]);
+
+ currentPlot.addMessageLine(pdpValues, ['currents[12]']).setColor(GREEN).setPointSize(0.0);
+ currentPlot.addMessageLine(pdpValues, ['currents[13]']).setColor(BLUE).setPointSize(0.0);
+ currentPlot.addMessageLine(status, ['shooter', 'finisher', 'commanded_current']).setColor(RED).setPointSize(0.0);
+}
diff --git a/y2020/control_loops/superstructure/hood_plotter.ts b/y2020/control_loops/superstructure/hood_plotter.ts
new file mode 100644
index 0000000..6c8025d
--- /dev/null
+++ b/y2020/control_loops/superstructure/hood_plotter.ts
@@ -0,0 +1,48 @@
+// 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 {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
+
+import Connection = proxy.Connection;
+
+const TIME = AosPlotter.TIME;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
+
+export function plotHood(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 robotState = aosPlotter.addMessageSource('/aos', 'aos.RobotState');
+
+ var currentTop = 0;
+
+ // Robot Enabled/Disabled and Mode
+ const positionPlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ positionPlot.plot.getAxisLabels().setTitle('Position');
+ positionPlot.plot.getAxisLabels().setXLabel(TIME);
+ positionPlot.plot.getAxisLabels().setYLabel('rad');
+ positionPlot.plot.setDefaultYRange([-0.1, 0.7]);
+
+ positionPlot.addMessageLine(goal, ['hood', 'unsafe_goal']).setColor(BLUE).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'goal_position']).setColor(RED).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'position']).setColor(GREEN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'velocity']).setColor(PINK).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'calculated_velocity']).setColor(BROWN).setPointSize(0.0);
+ positionPlot.addMessageLine(status, ['hood', 'estimator_state', 'position']).setColor(CYAN).setPointSize(0.0);
+
+ const voltagePlot =
+ aosPlotter.addPlot(element, [0, currentTop], [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
+ currentTop += DEFAULT_HEIGHT / 2;
+ voltagePlot.plot.getAxisLabels().setTitle('Voltage');
+ voltagePlot.plot.getAxisLabels().setXLabel(TIME);
+ voltagePlot.plot.getAxisLabels().setYLabel('Volts');
+ voltagePlot.plot.setDefaultYRange([-4.0, 16.0]);
+
+ voltagePlot.addMessageLine(output, ['hood_voltage']).setColor(BLUE).setPointSize(0.0);
+ voltagePlot.addMessageLine(status, ['hood', 'voltage_error']).setColor(RED).setPointSize(0.0);
+ voltagePlot.addMessageLine(robotState, ['voltage_battery']).setColor(GREEN).setPointSize(0.0);
+}
diff --git a/y2020/control_loops/superstructure/shooter/shooter.cc b/y2020/control_loops/superstructure/shooter/shooter.cc
index 6e48a21..8cdcf8c 100644
--- a/y2020/control_loops/superstructure/shooter/shooter.cc
+++ b/y2020/control_loops/superstructure/shooter/shooter.cc
@@ -30,8 +30,10 @@
std::abs(goal->velocity_accelerator() -
accelerator_left_.avg_angular_velocity()) < kVelocityTolerance &&
std::abs(goal->velocity_accelerator() -
- accelerator_right_.avg_angular_velocity()) < kVelocityTolerance &&
- std::abs(goal->velocity_finisher() - finisher_.velocity()) < kVelocityTolerance &&
+ accelerator_right_.avg_angular_velocity()) <
+ kVelocityTolerance &&
+ std::abs(goal->velocity_finisher() - finisher_.velocity()) <
+ kVelocityTolerance &&
std::abs(goal->velocity_accelerator() - accelerator_left_.velocity()) <
kVelocityTolerance &&
std::abs(goal->velocity_accelerator() - accelerator_right_.velocity()) <
diff --git a/y2020/control_loops/superstructure/superstructure.cc b/y2020/control_loops/superstructure/superstructure.cc
index e6c1375..c408f67 100644
--- a/y2020/control_loops/superstructure/superstructure.cc
+++ b/y2020/control_loops/superstructure/superstructure.cc
@@ -139,17 +139,6 @@
output_struct.turret_voltage =
std::clamp(output_struct.turret_voltage, -turret_.operating_voltage(),
turret_.operating_voltage());
-
- // Friction is a pain and putting a really high burden on the integrator.
- const double hood_velocity_sign =
- hood_status->velocity() * kHoodFrictionGain;
- output_struct.hood_voltage +=
- std::clamp(hood_velocity_sign, -kHoodFrictionVoltageLimit,
- kHoodFrictionVoltageLimit);
-
- // And dither the output.
- time_ += 0.00505;
- output_struct.hood_voltage += 1.3 * std::sin(time_ * 2.0 * M_PI * 30.0);
}
bool zeroed;
diff --git a/y2020/control_loops/superstructure/superstructure.h b/y2020/control_loops/superstructure/superstructure.h
index c0f3798..af15be6 100644
--- a/y2020/control_loops/superstructure/superstructure.h
+++ b/y2020/control_loops/superstructure/superstructure.h
@@ -29,9 +29,6 @@
static constexpr double kTurretFrictionGain = 10.0;
static constexpr double kTurretFrictionVoltageLimit = 1.5;
- static constexpr double kHoodFrictionGain = 40.0;
- static constexpr double kHoodFrictionVoltageLimit = 1.5;
-
using PotAndAbsoluteEncoderSubsystem =
::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
@@ -73,8 +70,6 @@
aos::monotonic_clock::time_point shooting_start_time_ =
aos::monotonic_clock::min_time;
- double time_ = 0.0;
-
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2020/control_loops/superstructure/superstructure_lib_test.cc b/y2020/control_loops/superstructure/superstructure_lib_test.cc
index 151146f..d6f5532 100644
--- a/y2020/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2020/control_loops/superstructure/superstructure_lib_test.cc
@@ -261,14 +261,9 @@
voltage_check_turret);
// Invert the friction model.
- const double hood_velocity_sign =
- hood_plant_->X(1) * Superstructure::kHoodFrictionGain;
::Eigen::Matrix<double, 1, 1> hood_U;
hood_U << superstructure_output_fetcher_->hood_voltage() +
- hood_plant_->voltage_offset() -
- std::clamp(hood_velocity_sign,
- -Superstructure::kHoodFrictionVoltageLimit,
- Superstructure::kHoodFrictionVoltageLimit);
+ hood_plant_->voltage_offset();
::Eigen::Matrix<double, 1, 1> intake_U;
intake_U << superstructure_output_fetcher_->intake_joint_voltage() +
diff --git a/y2020/joystick_reader.cc b/y2020/joystick_reader.cc
index 4528298..0a37df6 100644
--- a/y2020/joystick_reader.cc
+++ b/y2020/joystick_reader.cc
@@ -141,7 +141,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
hood_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
*builder.fbb(), hood_pos,
- CreateProfileParameters(*builder.fbb(), 0.7, 3.0));
+ CreateProfileParameters(*builder.fbb(), 5.0, 30.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
intake_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
diff --git a/y2020/wpilib_interface.cc b/y2020/wpilib_interface.cc
index e7b7b86..555d16b 100644
--- a/y2020/wpilib_interface.cc
+++ b/y2020/wpilib_interface.cc
@@ -262,7 +262,7 @@
// Shooter
y2020::control_loops::superstructure::ShooterPositionT shooter;
shooter.theta_finisher =
- encoder_translate(finisher_encoder_->GetRaw(),
+ encoder_translate(-finisher_encoder_->GetRaw(),
Values::kFinisherEncoderCountsPerRevolution(),
Values::kFinisherEncoderRatio());
@@ -377,8 +377,12 @@
accelerator_right_falcon_ = ::std::move(t);
}
- void set_flywheel_falcon(::std::unique_ptr<::frc::TalonFX> t) {
- finisher_falcon_ = ::std::move(t);
+ void set_finisher_falcon0(::std::unique_ptr<::frc::TalonFX> t) {
+ finisher_falcon0_ = ::std::move(t);
+ }
+
+ void set_finisher_falcon1(::std::unique_ptr<::frc::TalonFX> t) {
+ finisher_falcon1_ = ::std::move(t);
}
void set_climber_falcon(
@@ -391,9 +395,9 @@
private:
void Write(const superstructure::Output &output) override {
- hood_victor_->SetSpeed(
- std::clamp(output.hood_voltage(), -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ hood_victor_->SetSpeed(std::clamp(-output.hood_voltage(), -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
intake_joint_victor_->SetSpeed(std::clamp(-output.intake_joint_voltage(),
-kMaxBringupPower,
@@ -414,10 +418,12 @@
-kMaxBringupPower, kMaxBringupPower) /
12.0);
- washing_machine_control_panel_victor_->SetSpeed(
- std::clamp(-output.washing_machine_spinner_voltage(), -kMaxBringupPower,
- kMaxBringupPower) /
- 12.0);
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetSpeed(
+ std::clamp(-output.washing_machine_spinner_voltage(),
+ -kMaxBringupPower, kMaxBringupPower) /
+ 12.0);
+ }
accelerator_left_falcon_->SetSpeed(
std::clamp(-output.accelerator_left_voltage(), -kMaxBringupPower,
@@ -429,9 +435,14 @@
kMaxBringupPower) /
12.0);
- finisher_falcon_->SetSpeed(std::clamp(output.finisher_voltage(),
- -kMaxBringupPower, kMaxBringupPower) /
- 12.0);
+ finisher_falcon1_->SetSpeed(std::clamp(output.finisher_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
+ finisher_falcon0_->SetSpeed(std::clamp(-output.finisher_voltage(),
+ -kMaxBringupPower,
+ kMaxBringupPower) /
+ 12.0);
if (climber_falcon_) {
climber_falcon_->Set(
@@ -448,17 +459,20 @@
intake_joint_victor_->SetDisabled();
turret_victor_->SetDisabled();
feeder_falcon_->SetDisabled();
- washing_machine_control_panel_victor_->SetDisabled();
+ if (washing_machine_control_panel_victor_) {
+ washing_machine_control_panel_victor_->SetDisabled();
+ }
accelerator_left_falcon_->SetDisabled();
accelerator_right_falcon_->SetDisabled();
- finisher_falcon_->SetDisabled();
+ finisher_falcon0_->SetDisabled();
+ finisher_falcon1_->SetDisabled();
}
::std::unique_ptr<::frc::VictorSP> hood_victor_, intake_joint_victor_,
turret_victor_, washing_machine_control_panel_victor_;
::std::unique_ptr<::frc::TalonFX> feeder_falcon_, accelerator_left_falcon_,
- accelerator_right_falcon_, finisher_falcon_;
+ accelerator_right_falcon_, finisher_falcon0_, finisher_falcon1_;
::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
intake_roller_falcon_, climber_falcon_;
@@ -555,13 +569,15 @@
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
superstructure_writer.set_turret_victor(make_unique<frc::VictorSP>(7));
superstructure_writer.set_feeder_falcon(make_unique<frc::TalonFX>(6));
- superstructure_writer.set_washing_machine_control_panel_victor(
- make_unique<frc::VictorSP>(3));
+ // TODO(austin): When this goes over to CAN, update it and make it work.
+ //superstructure_writer.set_washing_machine_control_panel_victor(
+ //make_unique<frc::VictorSP>(3));
superstructure_writer.set_accelerator_left_falcon(
make_unique<::frc::TalonFX>(5));
superstructure_writer.set_accelerator_right_falcon(
make_unique<::frc::TalonFX>(4));
- superstructure_writer.set_flywheel_falcon(make_unique<::frc::TalonFX>(9));
+ superstructure_writer.set_finisher_falcon0(make_unique<::frc::TalonFX>(9));
+ superstructure_writer.set_finisher_falcon1(make_unique<::frc::TalonFX>(3));
// TODO: check port
superstructure_writer.set_climber_falcon(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));