diff --git a/index.html b/index.html
index d4e9dc3..1f5ceb5 100644
--- a/index.html
+++ b/index.html
@@ -51,7 +51,7 @@
-
+
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);