Merge "Core dump on SCHED_OTHER"
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index d223d13..8c81440 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -1,6 +1,6 @@
package(default_visibility = ["//visibility:public"])
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
load("//tools:environments.bzl", "mcu_cpus")
load("//tools/build_rules:select.bzl", "compiler_select", "cpu_select")
@@ -35,6 +35,12 @@
includes = ["//frc971/control_loops:control_loops_fbs_includes"],
)
+flatbuffer_ts_library(
+ name = "drivetrain_status_ts_fbs",
+ srcs = ["drivetrain_status.fbs"],
+ includes = ["//frc971/control_loops:control_loops_fbs_includes"],
+)
+
genrule(
name = "drivetrain_goal_float_fbs_generated",
srcs = ["drivetrain_goal.fbs"],
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 6b27c66..a8b0c67 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -26,6 +26,7 @@
deps = [
"//aos/network/www:proxy",
"//y2020/vision/sift:sift_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
],
)
diff --git a/y2020/www/field_handler.ts b/y2020/www/field_handler.ts
index 0d2ea34..6ec1afb 100644
--- a/y2020/www/field_handler.ts
+++ b/y2020/www/field_handler.ts
@@ -1,9 +1,11 @@
-import {Configuration, Channel} from 'aos/configuration_generated';
-import {Connection} from 'aos/network/www/proxy';
+import {Channel, Configuration} from 'aos/configuration_generated';
import {Connect} from 'aos/network/connect_generated';
-import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+import {Connection} from 'aos/network/www/proxy';
+import {Status as DrivetrainStatus} from 'frc971/control_loops/drivetrain/drivetrain_status_generated';
import {ImageMatchResult} from 'y2020/vision/sift/sift_generated'
+import {FIELD_LENGTH, FIELD_WIDTH, FT_TO_M, IN_TO_M} from './constants';
+
// (0,0) is field center, +X is toward red DS
const FIELD_SIDE_Y = FIELD_WIDTH / 2;
const FIELD_EDGE_X = FIELD_LENGTH / 2;
@@ -45,6 +47,9 @@
const TARGET_ZONE_WIDTH = 48 * IN_TO_M;
const LOADING_ZONE_WIDTH = 60 * IN_TO_M;
+const ROBOT_WIDTH = 28 * IN_TO_M;
+const ROBOT_LENGTH = 30 * IN_TO_M;
+
/**
* All the messages that are required to display camera information on the field.
* Messages not readable on the server node are ignored.
@@ -70,11 +75,16 @@
name: '/pi5/camera',
type: 'frc971.vision.sift.ImageMatchResult',
},
+ {
+ name: '/drivetrain',
+ type: 'frc971.control_loops.drivetrain.Status',
+ },
];
export class FieldHandler {
private canvas = document.createElement('canvas');
- private imageMatchResult :ImageMatchResult|null = null
+ private imageMatchResult: ImageMatchResult|null = null;
+ private drivetrainStatus: DrivetrianStatus|null = null;
constructor(private readonly connection: Connection) {
document.body.appendChild(this.canvas);
@@ -85,6 +95,9 @@
this.connection.addHandler(ImageMatchResult.getFullyQualifiedName(), (res) => {
this.handleImageMatchResult(res);
});
+ this.connection.addHandler(DrivetrainStatus.getFullyQualifiedName(), (data) => {
+ this.handleDrivetrainStatus(data);
+ });
}
private handleImageMatchResult(data: Uint8Array): void {
@@ -92,6 +105,11 @@
this.imageMatchResult = ImageMatchResult.getRootAsImageMatchResult(fbBuffer);
}
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new flatbuffers.ByteBuffer(data);
+ this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+ }
+
private sendConnect(): void {
const builder = new flatbuffers.Builder(512);
const channels: flatbuffers.Offset[] = [];
@@ -199,6 +217,20 @@
ctx.restore();
}
+ drawRobot(x: number, y: number, theta: number): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2, 0);
+ ctx.stroke();
+ ctx.restore();
+ }
+
draw(): void {
this.reset();
this.drawField();
@@ -209,11 +241,19 @@
const mat = pose.fieldToCamera();
const x = mat.data(3);
const y = mat.data(7);
- this.drawCamera(x, y, 0);
- console.log(x, y);
+ const theta = Math.atan2(
+ -mat.data(8),
+ Math.sqrt(Math.pow(mat.data(9), 2) + Math.pow(mat.data(10), 2)));
+ this.drawCamera(x, y, theta);
}
}
+ if (this.drivetrainStatus) {
+ this.drawRobot(
+ this.drivetrainStatus.x(), this.drivetrainStatus.y(),
+ this.drivetrainStatus.theta());
+ }
+
window.requestAnimationFrame(() => this.draw());
}
diff --git a/y2020/y2020.json b/y2020/y2020.json
index 81cf30c..df551e1 100644
--- a/y2020/y2020.json
+++ b/y2020/y2020.json
@@ -293,7 +293,24 @@
"source_node": "roborio",
"frequency": 200,
"max_size": 2000,
- "num_senders": 2
+ "num_senders": 2,
+ "destination_nodes": [
+ {
+ "name": "pi1",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "pi2",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "pi3",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/drivetrain",