Add Drivetrain to y2024 Webpage
Signed-off-by: Mirabel Wang <mirabel.17.wang@gmail.com>
Change-Id: I9e657ba73b72d86f6e50d11880a12cc1dcdc4d44
diff --git a/y2024/www/field.html b/y2024/www/field.html
index 8c3b291..ac94060 100644
--- a/y2024/www/field.html
+++ b/y2024/www/field.html
@@ -81,6 +81,22 @@
<td> Right Encoder Position</td>
<td id="right_drivetrain_encoder"> NA </td>
</tr>
+ <tr>
+ <td> Right Front Falcon CAN Position</td>
+ <td id="falcon_right_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Back Falcon CAN Position</td>
+ <td id="falcon_right_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Front Falcon CAN Position</td>
+ <td id="falcon_left_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Back Falcon CAN Position</td>
+ <td id="falcon_left_back"> NA </td>
+ </tr>
</table>
</body>
</html>
diff --git a/y2024/www/field_handler.ts b/y2024/www/field_handler.ts
index f383c08..81050dd 100644
--- a/y2024/www/field_handler.ts
+++ b/y2024/www/field_handler.ts
@@ -21,11 +21,75 @@
export class FieldHandler {
private canvas = document.createElement('canvas');
+ private localizerOutput: LocalizerOutput|null = null;
+ private drivetrainStatus: DrivetrainStatus|null = null;
+ private drivetrainPosition: DrivetrainPosition|null = null;
+ private drivetrainCANPosition: DrivetrainCANPosition|null = null;
+
+ private x: HTMLElement = (document.getElementById('x') as HTMLElement);
+ private y: HTMLElement = (document.getElementById('y') as HTMLElement);
+ private theta: HTMLElement =
+ (document.getElementById('theta') as HTMLElement);
+
private fieldImage: HTMLImageElement = new Image();
+
+ private leftDrivetrainEncoder: HTMLElement =
+ (document.getElementById('left_drivetrain_encoder') as HTMLElement);
+ private rightDrivetrainEncoder: HTMLElement =
+ (document.getElementById('right_drivetrain_encoder') as HTMLElement);
+ private falconRightFrontPosition: HTMLElement =
+ (document.getElementById('falcon_right_front') as HTMLElement);
+ private falconRightBackPosition: HTMLElement =
+ (document.getElementById('falcon_right_back') as HTMLElement);
+ private falconLeftFrontPosition: HTMLElement =
+ (document.getElementById('falcon_left_front') as HTMLElement);
+ private falconLeftBackPosition: HTMLElement =
+ (document.getElementById('falcon_left_back') as HTMLElement);
+
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
this.fieldImage.src = '2024.png';
+
+ this.connection.addConfigHandler(() => {
+
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status', (data) => {
+ this.handleDrivetrainStatus(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Position', (data) => {
+ this.handleDrivetrainPosition(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.CANPosition', (data) => {
+ this.handleDrivetrainCANPosition(data);
+ });
+ this.connection.addHandler(
+ '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
+ this.handleLocalizerOutput(data);
+ });
+ });
+ }
+
+ private handleDrivetrainStatus(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
+ }
+
+ private handleDrivetrainPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainPosition = DrivetrainPosition.getRootAsPosition(fbBuffer);
+ }
+
+ private handleDrivetrainCANPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainCANPosition = DrivetrainCANPosition.getRootAsCANPosition(fbBuffer);
+ }
+
+ private handleLocalizerOutput(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.localizerOutput = LocalizerOutput.getRootAsLocalizerOutput(fbBuffer);
}
drawField(): void {
@@ -38,10 +102,95 @@
ctx.restore();
}
+ drawRobot(
+ x: number, y: number, theta: number, color: string = 'blue',
+ dashed: boolean = false): void {
+ const ctx = this.canvas.getContext('2d');
+ ctx.save();
+ ctx.translate(x, y);
+ ctx.rotate(theta);
+ ctx.strokeStyle = color;
+ ctx.lineWidth = ROBOT_WIDTH / 10.0;
+ if (dashed) {
+ ctx.setLineDash([0.05, 0.05]);
+ } else {
+ // Empty array = solid line.
+ ctx.setLineDash([]);
+ }
+ ctx.rect(-ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2, ROBOT_LENGTH, ROBOT_WIDTH);
+ ctx.stroke();
+
+ // Draw line indicating which direction is forwards on the robot.
+ ctx.beginPath();
+ ctx.moveTo(0, 0);
+ ctx.lineTo(ROBOT_LENGTH / 2.0, 0);
+ ctx.stroke();
+
+ ctx.restore();
+}
+
+ setZeroing(div: HTMLElement): void {
+ div.innerHTML = 'zeroing';
+ div.classList.remove('faulted');
+ div.classList.add('zeroing');
+ div.classList.remove('near');
+}
+
+ setValue(div: HTMLElement, val: number): void {
+ div.innerHTML = val.toFixed(4);
+ div.classList.remove('faulted');
+ div.classList.remove('zeroing');
+ div.classList.remove('near');
+}
draw(): void {
this.reset();
this.drawField();
+ if (this.drivetrainPosition) {
+ this.leftDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.leftEncoder().toString();
+
+ this.rightDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.rightEncoder().toString();
+ }
+
+ if (this.drivetrainCANPosition) {
+ this.falconRightFrontPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(0).position().toString();
+
+ this.falconRightBackPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(1).position().toString();
+
+ this.falconLeftFrontPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(2).position().toString();
+
+ this.falconLeftBackPosition.innerHTML =
+ this.drivetrainCANPosition.talonfxs(3).position().toString();
+ }
+
+ if (this.drivetrainStatus && this.drivetrainStatus.trajectoryLogging()) {
+ this.drawRobot(
+ this.drivetrainStatus.trajectoryLogging().x(),
+ this.drivetrainStatus.trajectoryLogging().y(),
+ this.drivetrainStatus.trajectoryLogging().theta(), '#000000FF',
+ false);
+ }
+
+ if (this.localizerOutput) {
+ if (!this.localizerOutput.zeroed()) {
+ this.setZeroing(this.x);
+ this.setZeroing(this.y);
+ this.setZeroing(this.theta);
+ } else {
+ this.setValue(this.x, this.localizerOutput.x());
+ this.setValue(this.y, this.localizerOutput.y());
+ this.setValue(this.theta, this.localizerOutput.theta());
+ }
+
+ this.drawRobot(
+ this.localizerOutput.x(), this.localizerOutput.y(),
+ this.localizerOutput.theta());
+ }
window.requestAnimationFrame(() => this.draw());
}