Munelit/swerve-visualizer
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Gyro heading added to swerve drive structure and implemented it to the visualization

Browse Source
This commit is contained in:
Moonlit Productions
2025-10-29 10:00:49 -04:00
parent f91374ed64
commit 9bd249af6f
2 changed files with 52 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
index.html
View File

@ -92,6 +92,8 @@
<div id="current-config-info" class="config-info">
Current Configuration: <strong id="config-name">4-Wheel Rectangle</strong>
(<span id="module-count-display">4</span> modules)
<br>
Gyro Heading: <strong id="gyro-heading-display">0.0°</strong>
</div>
<div class="module-grid" id="module-grid">
<!-- Dynamically generated module data will appear here -->

75
script.js
View File

@ -28,19 +28,27 @@ class SwerveModule {
this.name = name;
}
calculateState(velocityX, velocityY, turnSpeed) {
calculateState(velocityX, velocityY, turnSpeed, heading = 0) {
// Take the requested speed and turn rate of the robot and calculate
// speed and angle of this module to achieve it
// Transform field-relative velocities to robot-relative velocities
// by rotating the velocity vector by the negative of the robot's heading
const cosHeading = Math.cos(-heading);
const sinHeading = Math.sin(-heading);
const robotVelX = velocityX * cosHeading - velocityY * sinHeading;
const robotVelY = velocityX * sinHeading + velocityY * cosHeading;
// Calculate rotation contribution (perpendicular to position vector)
const rotX = -this.position.y * turnSpeed;
const rotY = this.position.x * turnSpeed;
// Combine translation and rotation
this.velocity.x = velocityX + rotX;
this.velocity.y = velocityY + rotY;
// Combine translation and rotation (now in robot frame)
this.velocity.x = robotVelX + rotX;
this.velocity.y = robotVelY + rotY;
// Calculate speed and angle
// Calculate speed and angle (in robot frame)
this.speed = this.velocity.magnitude();
this.angle = this.velocity.angle();
}
@ -51,6 +59,7 @@ class SwerveDrive {
constructor(modulePositionsAndNames, robotName) {
this.setModules(modulePositionsAndNames);
this.setName(robotName);
this.gyroHeading = 0; // Simulated gyro heading in radians
}
setName(robotName) {
@ -64,10 +73,23 @@ class SwerveDrive {
);
}
drive(velocityX, velocityY, turnSpeed, maxModuleSpeed) {
updateHeading(turnSpeed, deltaTime = 0.01) {
// Integrate turn speed to update gyro heading
// turnSpeed is in radians/second, deltaTime is the time step
this.gyroHeading += turnSpeed * deltaTime;
// Normalize to -PI to PI range
while (this.gyroHeading > Math.PI) this.gyroHeading -= 2 * Math.PI;
while (this.gyroHeading < -Math.PI) this.gyroHeading += 2 * Math.PI;
}
drive(velocityX, velocityY, turnSpeed, maxModuleSpeed, deltaTime = 0.01) {
// Update gyro heading first
this.updateHeading(turnSpeed, deltaTime);
// Take in a requested speeds and update every module
this.modules.forEach(module =>
module.calculateState(velocityX, velocityY, turnSpeed)
module.calculateState(velocityX, velocityY, turnSpeed, this.gyroHeading)
);
// If any speeds exceed the max speed, normalize down so we don't effect movement direction
@ -369,6 +391,13 @@ function updateModuleDisplays(robot) {
const moduleCount = document.getElementById('module-count-display');
moduleCount.textContent = robot.modules.length;
// Update gyro heading display
const gyroHeadingDisplay = document.getElementById('gyro-heading-display');
if (gyroHeadingDisplay) {
const headingDeg = (robot.gyroHeading * 180 / Math.PI).toFixed(1);
gyroHeadingDisplay.textContent = `${headingDeg}°`;
}
const modules = robot.modules;
modules.forEach((module, i) => {
const angleElement = document.getElementById(`module-${i}-angle`);
@ -394,12 +423,9 @@ const ctx = canvas.getContext('2d');
// Get CSS variables for use in canvas
const rootStyles = getComputedStyle(document.documentElement);
function drawGrid(ctx, sideLength, gridSquareSize, xOffset, yOffset, rotation) {
function drawGrid(ctx, sideLength, gridSquareSize, xOffset, yOffset) {
ctx.save();
// Apply rotation transform
ctx.rotate(-rotation);
ctx.strokeStyle = rootStyles.getPropertyValue('--grid-color');
ctx.lineWidth = 1;
const startX = (-sideLength / 2) - xOffset;
@ -463,7 +489,11 @@ function drawModule(ctx, module) {
ctx.restore();
}
function drawRobot(ctx, robot) {
function drawRobot(ctx, robot, heading) {
ctx.save(); // Save current state before rotation
ctx.rotate(heading);
ctx.strokeStyle = rootStyles.getPropertyValue('--robot-frame-color')
ctx.fillStyle = rootStyles.getPropertyValue('--robot-fill-color');
ctx.lineWidth = 4;
@ -480,6 +510,8 @@ function drawRobot(ctx, robot) {
ctx.stroke();
modules.forEach(module => drawModule(ctx, module));
ctx.restore(); // Restore to remove rotation
}
@ -491,9 +523,8 @@ createModuleDisplays(robot);
let xSpeed = 0;
let ySpeed = 0;
let turnSpeed = -1;
let robotRotation = 0; // Track cumulative robot rotation for grid display
let gridSquareSize = 25;
let gridSquareSize = 50;
let xGridOffset = 0;
let yGridOffset = 0;
robot.drive(xSpeed, ySpeed, 0, 500);
@ -511,26 +542,20 @@ function animate() {
// Animate the grid with robot movement
let offsetSpeedDivisor = (100 - gridSquareSize <= 0 ? 1 : 100 - gridSquareSize);
robotRotation += turnSpeed * 0.01; // Scale factor for reasonable rotation speed
// Convert robot velocities to world velocities for grid movement
const cosRot = Math.cos(robotRotation);
const sinRot = Math.sin(robotRotation);
const worldVx = xSpeed * cosRot - ySpeed * sinRot;
const worldVy = xSpeed * sinRot + ySpeed * cosRot;
// Update grid offsets based on robot movement
xGridOffset = (xGridOffset + (worldVx / offsetSpeedDivisor)) % gridSquareSize;
yGridOffset = (yGridOffset + (worldVy / offsetSpeedDivisor)) % gridSquareSize;
xGridOffset = (xGridOffset + (xSpeed / offsetSpeedDivisor)) % gridSquareSize;
yGridOffset = (yGridOffset + (ySpeed / offsetSpeedDivisor)) % gridSquareSize;
// Update module states before drawing the robot
// The drive() method will update the gyroHeading internally
robot.drive(xSpeed, ySpeed, turnSpeed, parseFloat(maxSpeedSlider.value));
updateModuleDisplays(robot);
// Draw the robot and it's movement. Grid should be oversized so movement
// doesn't find the edge of the grid
drawGrid(ctx, canvas.width * 2, gridSquareSize, xGridOffset, yGridOffset, robotRotation);
drawRobot(ctx, robot);
drawGrid(ctx, canvas.width * 2, gridSquareSize, xGridOffset, yGridOffset);
drawRobot(ctx, robot, robot.gyroHeading);
// Do it all over again
ctx.restore();