commit 76f47565c098fad54faf0fee9011f1d2f77bd2ae
author Niko Sohmers <nikolai@sohmers.com> Wed Dec 20 20:59:06 2023 -0800
committer Niko Sohmers <nikolai@sohmers.com> Wed Dec 20 20:59:06 2023 -0800
tree b346ec306c30ed7a46b6ed968cec7ea6fbcc8994
parent 0be96fb5bb397c03a2b46761ec291c84b46ca8c6

Add Drivetrain Position and CANPosition to Logging Site

Signed-off-by: Niko Sohmers <nikolai@sohmers.com>
Change-Id: I55def05b6d274ba54d07eec766cf12177135e1d9

y2023/www/field_handler.ts

diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 6952a41..b478538 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -3,6 +3,8 @@
 import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
 import {Connection} from '../../aos/network/www/proxy'
 import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
+import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
 import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
 import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
 import {TargetMap} from '../../frc971/vision/target_map_generated'
@@ -27,6 +29,8 @@
   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 superstructureStatus: SuperstructureStatus|null = null;
 
   // Image information indexed by timestamp (seconds since the epoch), so that
@@ -70,6 +74,24 @@
       (document.getElementById('arm_distal') as HTMLElement);
   private zeroingFaults: HTMLElement =
       (document.getElementById('zeroing_faults') as HTMLElement);
+
+  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 falconRightUnderPosition: HTMLElement =
+      (document.getElementById('falcon_right_under') as HTMLElement);
+  private falconLeftFrontPosition: HTMLElement =
+      (document.getElementById('falcon_left_front') as HTMLElement);
+  private falconLeftBackPosition: HTMLElement =
+      (document.getElementById('falcon_left_back') as HTMLElement);
+  private falconLeftUnderPosition: HTMLElement =
+      (document.getElementById('falcon_left_under') as HTMLElement);
+
   constructor(private readonly connection: Connection) {
     (document.getElementById('field') as HTMLElement).appendChild(this.canvas);
 
@@ -155,6 +177,14 @@
             this.handleDrivetrainStatus(data);
           });
       this.connection.addHandler(
+          '/drivetrain', 'frc971.control_loops.drivetrain.Position', (data) => {
+            this.handleDrivetrainPosition(data);
+          });
+      this.connection.addHandler(
+          '/drivetrain', 'y2023.control_loops.drivetrain.CANPosition', (data) => {
+            this.handleDrivetrainCANPosition(data);
+          });
+      this.connection.addHandler(
           '/localizer', 'frc971.controls.LocalizerOutput', (data) => {
             this.handleLocalizerOutput(data);
           });
@@ -210,6 +240,16 @@
     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 handleSuperstructureStatus(data: Uint8Array): void {
     const fbBuffer = new ByteBuffer(data);
     this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
@@ -463,6 +503,34 @@
           false);
     }
 
+    if (this.drivetrainPosition) {
+        this.leftDrivetrainEncoder.innerHTML =
+        this.drivetrainPosition.leftEncoder().toString();
+
+        this.rightDrivetrainEncoder.innerHTML =
+        this.drivetrainPosition.rightEncoder().toString();
+    }
+
+    if (this.drivetrainCANPosition) {
+      this.falconRightFrontPosition.innerHTML = //TODO: (niko) Improve this so that falcons are not hard-coded
+      this.drivetrainCANPosition.falcons(0).position().toString();
+
+      this.falconRightBackPosition.innerHTML =
+      this.drivetrainCANPosition.falcons(1).position().toString();
+
+      this.falconRightUnderPosition.innerHTML =
+      this.drivetrainCANPosition.falcons(2).position().toString();
+
+      this.falconLeftFrontPosition.innerHTML =
+      this.drivetrainCANPosition.falcons(3).position().toString();
+
+      this.falconLeftBackPosition.innerHTML =
+      this.drivetrainCANPosition.falcons(4).position().toString();
+
+      this.falconLeftUnderPosition.innerHTML =
+      this.drivetrainCANPosition.falcons(5).position().toString();
+  }
+
     if (this.localizerOutput) {
       if (!this.localizerOutput.zeroed()) {
         this.setZeroing(this.x);