Migrated visualization to use values calculated from modules to better visualize it

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);