From ba710fcf5d91541f54ba2e212089c93328fe4722 Mon Sep 17 00:00:00 2001 From: Moonlit Productions Date: Wed, 29 Oct 2025 14:01:59 -0400 Subject: [PATCH] Migrated visualization to use values calculated from modules to better visualize it --- index.html | 2 +- script.js | 84 +++++++++++++++++++++++++++++++++++------------------- 2 files changed, 56 insertions(+), 30 deletions(-) diff --git a/index.html b/index.html index d4e9dc3..1f5ceb5 100644 --- a/index.html +++ b/index.html @@ -51,7 +51,7 @@
- + 0
diff --git a/script.js b/script.js index 8dcd7b5..66772f5 100644 --- a/script.js +++ b/script.js @@ -86,18 +86,19 @@ class SwerveDrive { } drive(velocityX, velocityY, turnSpeed, maxModuleSpeed, deltaTime = 0.01) { - // Update gyro heading first - this.updateHeading(turnSpeed, deltaTime); + // Store the requested turn speed for later calculation of actual turn speed + this.requestedTurnSpeed = turnSpeed; - // Take in a requested speeds and update every module + // Take in a requested speeds and update every module (but don't update heading yet) this.modules.forEach(module => module.calculateState(velocityX, velocityY, turnSpeed, this.gyroHeading) ); // If any speeds exceed the max speed, normalize down so we don't effect movement direction const maxCalculated = Math.max(...this.modules.map(m => m.speed), 0); + let scale = 1.0; if (maxCalculated > maxModuleSpeed) { - const scale = maxModuleSpeed / maxCalculated; + scale = maxModuleSpeed / maxCalculated; this.modules.forEach(module => { module.velocity.x *= scale; module.velocity.y *= scale; @@ -105,6 +106,38 @@ class SwerveDrive { module.angle = module.velocity.angle(); }); } + + // Update heading with the actual turn speed (scaled if modules were limited) + const actualTurnSpeed = turnSpeed * scale; + this.updateHeading(actualTurnSpeed, deltaTime); + this.actualTurnSpeed = actualTurnSpeed; + } + + getActualVelocity() { + // Calculate the actual robot velocity from the average of module velocities + // This returns the velocity in robot-relative coordinates + if (this.modules.length === 0) return new Vec2D(0, 0); + + let sumX = 0; + let sumY = 0; + + // Average the module velocities (they're in robot frame) + this.modules.forEach(module => { + sumX += module.velocity.x; + sumY += module.velocity.y; + }); + + const avgX = sumX / this.modules.length; + const avgY = sumY / this.modules.length; + + // Transform back to field-relative coordinates + const cosHeading = Math.cos(this.gyroHeading); + const sinHeading = Math.sin(this.gyroHeading); + + const fieldVelX = avgX * cosHeading - avgY * sinHeading; + const fieldVelY = avgX * sinHeading + avgY * cosHeading; + + return new Vec2D(fieldVelX, fieldVelY); } } @@ -271,26 +304,10 @@ const preset16OctBtn = document.getElementById('preset-16oct'); * BEGIN LISTENER CODE */ - -vxSlider.addEventListener('input', (e) => { - vxOutput.textContent = parseFloat(e.target.value); -}); -vxOutput.textContent = parseFloat(vxSlider.value); - -vySlider.addEventListener('input', (e) => { - vyOutput.textContent = parseFloat(e.target.value); -}); -vyOutput.textContent = parseFloat(vySlider.value); - -omegaSlider.addEventListener('input', (e) => { - omegaOutput.textContent = parseFloat(e.target.value); -}); -omegaOutput.textContent = parseFloat(omegaSlider.value); - maxSpeedSlider.addEventListener('input', (e) => { - maxSpeedOutput.textContent = parseFloat(e.target.value); + maxSpeedOutput.textContent = e.target.value; }); -maxSpeedOutput.textContent = parseFloat(maxSpeedSlider.value); +maxSpeedOutput.textContent = maxSpeedSlider.value; resetBtn.addEventListener('click', (e) => { vxSlider.value = 0; @@ -640,18 +657,27 @@ function animate() { ySpeed = -parseFloat(vySlider.value); turnSpeed = parseFloat(omegaSlider.value); - // Animate the grid with robot movement - let offsetSpeedDivisor = (100 - gridSquareSize <= 0 ? 1 : 100 - gridSquareSize); - - // Update grid offsets based on robot movement - 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); + // Get the actual robot velocity (after scaling to max module speed) for grid animation + const actualVelocity = robot.getActualVelocity(); + + + // Update control outputs with actual speeds + vxOutput.textContent = `Requested: ${vxSlider.value} | Actual: ${actualVelocity.x.toFixed(2)}`; + vyOutput.textContent = `Requested: ${vySlider.value} | Actual: ${-actualVelocity.y.toFixed(2)}`; + omegaOutput.textContent = `Requested: ${omegaSlider.value} | Actual: ${robot.actualTurnSpeed.toFixed(2)}`; + + // Animate the grid + let offsetSpeedDivisor = (100 - gridSquareSize <= 0 ? 1 : 100 - gridSquareSize); + + // Update grid offsets based on robot movement + xGridOffset = (xGridOffset + (actualVelocity.x / offsetSpeedDivisor)) % gridSquareSize; + yGridOffset = (yGridOffset + (actualVelocity.y / offsetSpeedDivisor)) % gridSquareSize; + // 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);