| import {ByteBuffer} from 'flatbuffers' |
| import {ClientStatistics} from '../../aos/network/message_bridge_client_generated' |
| 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 {SuperstructureState, IntakeRollerStatus, CatapultState, TransferRollerStatus, ExtendRollerStatus, ExtendStatus, Status as SuperstructureStatus} from '../control_loops/superstructure/superstructure_status_generated' |
| import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated' |
| import {TargetMap} from '../../frc971/vision/target_map_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; |
| |
| const ROBOT_WIDTH = 29 * IN_TO_M; |
| const ROBOT_LENGTH = 32 * IN_TO_M; |
| |
| 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 superstructureStatus: SuperstructureStatus|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 zeroingFaults: HTMLElement = |
| (document.getElementById('zeroing_faults') as HTMLElement); |
| |
| private superstructureState: HTMLElement = |
| (document.getElementById('superstructure_state') as HTMLElement); |
| |
| private intakeRollerState: HTMLElement = |
| (document.getElementById('intake_roller_state') as HTMLElement); |
| private transferRollerState: HTMLElement = |
| (document.getElementById('transfer_roller_state') as HTMLElement); |
| private extendState: HTMLElement = |
| (document.getElementById('extend_state') as HTMLElement); |
| private extendRollerState: HTMLElement = |
| (document.getElementById('extend_roller_state') as HTMLElement); |
| private catapultState: HTMLElement = |
| (document.getElementById('catapult_state') as HTMLElement); |
| |
| private intakePivot: HTMLElement = |
| (document.getElementById('intake_pivot') as HTMLElement); |
| private intakePivotAbs: HTMLElement = |
| (document.getElementById('intake_pivot_abs') as HTMLElement); |
| |
| private climber: HTMLElement = |
| (document.getElementById('climber') as HTMLElement); |
| private climberAbs: HTMLElement = |
| (document.getElementById('climber_abs') as HTMLElement); |
| private climberPot: HTMLElement = |
| (document.getElementById('climber_pot') as HTMLElement); |
| |
| private extend: HTMLElement = |
| (document.getElementById('extend') as HTMLElement); |
| private extendAbs: HTMLElement = |
| (document.getElementById('extend_abs') as HTMLElement); |
| private extendPot: HTMLElement = |
| (document.getElementById('extend_pot') as HTMLElement); |
| |
| private turret: HTMLElement = |
| (document.getElementById('turret') as HTMLElement); |
| private turretAbs: HTMLElement = |
| (document.getElementById('turret_abs') as HTMLElement); |
| private turretPot: HTMLElement = |
| (document.getElementById('turret_pot') as HTMLElement); |
| |
| private catapult: HTMLElement = |
| (document.getElementById('catapult') as HTMLElement); |
| private catapultAbs: HTMLElement = |
| (document.getElementById('catapult_abs') as HTMLElement); |
| private catapultPot: HTMLElement = |
| (document.getElementById('catapult_pot') as HTMLElement); |
| |
| private altitude: HTMLElement = |
| (document.getElementById('altitude') as HTMLElement); |
| private altitudeAbs: HTMLElement = |
| (document.getElementById('altitude_abs') as HTMLElement); |
| private altitudePot: HTMLElement = |
| (document.getElementById('altitude_pot') as HTMLElement); |
| |
| private turret_position: HTMLElement = |
| (document.getElementById('turret_position') as HTMLElement); |
| private turret_velocity: HTMLElement = |
| (document.getElementById('turret_velocity') as HTMLElement); |
| private target_distance: HTMLElement = |
| (document.getElementById('target_distance') as HTMLElement); |
| private shot_distance: HTMLElement = |
| (document.getElementById('shot_distance') 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 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); |
| }); |
| this.connection.addHandler( |
| '/superstructure', "y2024.control_loops.superstructure.Status", |
| (data) => { |
| this.handleSuperstructureStatus(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); |
| } |
| |
| private handleSuperstructureStatus(data: Uint8Array): void { |
| const fbBuffer = new ByteBuffer(data); |
| this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer); |
| } |
| |
| drawField(): void { |
| const ctx = this.canvas.getContext('2d'); |
| ctx.save(); |
| ctx.scale(1.0, -1.0); |
| ctx.drawImage( |
| this.fieldImage, 0, 0, this.fieldImage.width, this.fieldImage.height, |
| -FIELD_EDGE_X, -FIELD_SIDE_Y, FIELD_LENGTH, FIELD_WIDTH); |
| ctx.restore(); |
| } |
| |
| drawCamera(x: number, y: number, theta: number, color: string = 'blue'): |
| void { |
| const ctx = this.canvas.getContext('2d'); |
| ctx.save(); |
| ctx.translate(x, y); |
| ctx.rotate(theta); |
| ctx.strokeStyle = color; |
| ctx.beginPath(); |
| ctx.moveTo(0.5, 0.5); |
| ctx.lineTo(0, 0); |
| ctx.lineTo(0.5, -0.5); |
| ctx.stroke(); |
| ctx.beginPath(); |
| ctx.arc(0, 0, 0.25, -Math.PI / 4, Math.PI / 4); |
| ctx.stroke(); |
| 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'); |
| } |
| |
| setEstopped(div: HTMLElement): void { |
| div.innerHTML = 'estopped'; |
| div.classList.add('faulted'); |
| div.classList.remove('zeroing'); |
| div.classList.remove('near'); |
| } |
| |
| setTargetValue( |
| div: HTMLElement, target: number, val: number, tolerance: number): void { |
| div.innerHTML = val.toFixed(4); |
| div.classList.remove('faulted'); |
| div.classList.remove('zeroing'); |
| if (Math.abs(target - val) < tolerance) { |
| div.classList.add('near'); |
| } else { |
| 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.superstructureStatus) { |
| this.superstructureState.innerHTML = |
| SuperstructureState[this.superstructureStatus.state()]; |
| |
| this.intakeRollerState.innerHTML = |
| IntakeRollerStatus[this.superstructureStatus.intakeRoller()]; |
| this.transferRollerState.innerHTML = |
| TransferRollerStatus[this.superstructureStatus.transferRoller()]; |
| this.extendState.innerHTML = |
| ExtendStatus[this.superstructureStatus.extendStatus()]; |
| this.extendRollerState.innerHTML = |
| ExtendRollerStatus[this.superstructureStatus.extendRoller()]; |
| this.catapultState.innerHTML = |
| CatapultState[this.superstructureStatus.shooter().catapultState()]; |
| |
| if (this.superstructureStatus.shooter() && |
| this.superstructureStatus.shooter().aimer()) { |
| this.turret_position.innerHTML = this.superstructureStatus.shooter() |
| .aimer() |
| .turretPosition() |
| .toString(); |
| this.turret_velocity.innerHTML = this.superstructureStatus.shooter() |
| .aimer() |
| .turretVelocity() |
| .toString(); |
| this.target_distance.innerHTML = this.superstructureStatus.shooter() |
| .aimer() |
| .targetDistance() |
| .toString(); |
| this.shot_distance.innerHTML = this.superstructureStatus.shooter() |
| .aimer() |
| .shotDistance() |
| .toString(); |
| } |
| |
| if (!this.superstructureStatus.intakePivot() || |
| !this.superstructureStatus.intakePivot().zeroed()) { |
| this.setZeroing(this.intakePivot); |
| } else if (this.superstructureStatus.intakePivot().estopped()) { |
| this.setEstopped(this.intakePivot); |
| } else { |
| this.setTargetValue( |
| this.intakePivot, |
| this.superstructureStatus.intakePivot().unprofiledGoalPosition(), |
| this.superstructureStatus.intakePivot().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.intakePivotAbs.innerHTML = this.superstructureStatus.intakePivot().estimatorState().absolutePosition().toString(); |
| |
| if (!this.superstructureStatus.climber() || |
| !this.superstructureStatus.climber().zeroed()) { |
| this.setZeroing(this.climber); |
| } else if (this.superstructureStatus.climber().estopped()) { |
| this.setEstopped(this.climber); |
| } else { |
| this.setTargetValue( |
| this.climber, |
| this.superstructureStatus.climber().unprofiledGoalPosition(), |
| this.superstructureStatus.climber().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.climberAbs.innerHTML = this.superstructureStatus.climber().estimatorState().absolutePosition().toString(); |
| this.climberPot.innerHTML = this.superstructureStatus.climber().estimatorState().potPosition().toString(); |
| |
| if (!this.superstructureStatus.extend() || |
| !this.superstructureStatus.extend().zeroed()) { |
| this.setZeroing(this.extend); |
| } else if (this.superstructureStatus.extend().estopped()) { |
| this.setEstopped(this.extend); |
| } else { |
| this.setTargetValue( |
| this.extend, |
| this.superstructureStatus.extend().unprofiledGoalPosition(), |
| this.superstructureStatus.extend().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.extendAbs.innerHTML = this.superstructureStatus.extend().estimatorState().absolutePosition().toString(); |
| this.extendPot.innerHTML = this.superstructureStatus.extend().estimatorState().potPosition().toString(); |
| |
| if (!this.superstructureStatus.shooter().turret() || |
| !this.superstructureStatus.shooter().turret().zeroed()) { |
| this.setZeroing(this.turret); |
| } else if (this.superstructureStatus.shooter().turret().estopped()) { |
| this.setEstopped(this.turret); |
| } else { |
| this.setTargetValue( |
| this.turret, |
| this.superstructureStatus.shooter().turret().unprofiledGoalPosition(), |
| this.superstructureStatus.shooter().turret().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.turretAbs.innerHTML = this.superstructureStatus.shooter().turret().estimatorState().absolutePosition().toString(); |
| this.turretPot.innerHTML = this.superstructureStatus.shooter().turret().estimatorState().potPosition().toString(); |
| |
| if (!this.superstructureStatus.shooter().catapult() || |
| !this.superstructureStatus.shooter().catapult().zeroed()) { |
| this.setZeroing(this.catapult); |
| } else if (this.superstructureStatus.shooter().catapult().estopped()) { |
| this.setEstopped(this.catapult); |
| } else { |
| this.setTargetValue( |
| this.catapult, |
| this.superstructureStatus.shooter().catapult().unprofiledGoalPosition(), |
| this.superstructureStatus.shooter().catapult().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.catapultAbs.innerHTML = this.superstructureStatus.shooter().catapult().estimatorState().absolutePosition().toString(); |
| this.catapultPot.innerHTML = this.superstructureStatus.shooter().catapult().estimatorState().potPosition().toString(); |
| |
| if (!this.superstructureStatus.shooter().altitude() || |
| !this.superstructureStatus.shooter().altitude().zeroed()) { |
| this.setZeroing(this.altitude); |
| } else if (this.superstructureStatus.shooter().altitude().estopped()) { |
| this.setEstopped(this.altitude); |
| } else { |
| this.setTargetValue( |
| this.altitude, |
| this.superstructureStatus.shooter().altitude().unprofiledGoalPosition(), |
| this.superstructureStatus.shooter().altitude().estimatorState().position(), |
| 1e-3); |
| } |
| |
| this.altitudeAbs.innerHTML = this.superstructureStatus.shooter().altitude().estimatorState().absolutePosition().toString(); |
| this.altitudePot.innerHTML = this.superstructureStatus.shooter().altitude().estimatorState().potPosition().toString(); |
| |
| let zeroingErrors: string = 'Intake Pivot Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.intakePivot() |
| .estimatorState() |
| .errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.intakePivot() |
| .estimatorState() |
| .errors(i)] + |
| '<br/>'; |
| } |
| zeroingErrors += '<br/>' + |
| 'Climber Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.climber().estimatorState().errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.climber().estimatorState().errors(i)] + |
| '<br/>'; |
| } |
| zeroingErrors += '<br/>' + |
| 'Extend Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.extend().estimatorState().errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.extend().estimatorState().errors(i)] + |
| '<br/>'; |
| } |
| zeroingErrors += '<br/>' + |
| 'Turret Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.shooter().turret().estimatorState().errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.shooter().turret().estimatorState().errors(i)] + |
| '<br/>'; |
| } |
| zeroingErrors += '<br/>' + |
| 'Catapult Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.shooter().catapult().estimatorState().errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.shooter().catapult().estimatorState().errors(i)] + |
| '<br/>'; |
| } |
| zeroingErrors += '<br/>' + |
| 'Altitude Errors:' + |
| '<br/>'; |
| for (let i = 0; i < this.superstructureStatus.shooter().altitude().estimatorState().errorsLength(); |
| i++) { |
| zeroingErrors += ZeroingError[this.superstructureStatus.shooter().altitude().estimatorState().errors(i)] + |
| '<br/>'; |
| } |
| this.zeroingFaults.innerHTML = zeroingErrors; |
| } |
| |
| 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()); |
| } |
| |
| reset(): void { |
| const ctx = this.canvas.getContext('2d'); |
| // Empty space from the canvas boundary to the image |
| const IMAGE_PADDING = 10; |
| ctx.setTransform(1, 0, 0, 1, 0, 0); |
| const size = window.innerHeight * 0.9; |
| ctx.canvas.height = size; |
| const width = size / 2 + 20; |
| ctx.canvas.width = width; |
| ctx.clearRect(0, 0, size, width); |
| |
| // Translate to center of display. |
| ctx.translate(width / 2, size / 2); |
| // Coordinate system is: |
| // x -> forward. |
| // y -> to the left. |
| ctx.rotate(-Math.PI / 2); |
| ctx.scale(1, -1); |
| |
| const M_TO_PX = (size - IMAGE_PADDING) / FIELD_LENGTH; |
| ctx.scale(M_TO_PX, M_TO_PX); |
| ctx.lineWidth = 1 / M_TO_PX; |
| } |
| } |