diff --git a/script.js b/script.js index 574a548..f3d830e 100644 --- a/script.js +++ b/script.js @@ -27,7 +27,18 @@ class SwerveModule { calculateState(velocityX, velocityY, turnSpeed) { // Take the requested speed and turn rate of the robot and calculate // speed and angle of this module to achieve it - throw new Error("Not Yet Implemented"); + + // Calculate rotation contribution (perpendicular to position vector) + const rotX = -this.position.y * omega; + const rotY = this.position.x * omega; + + // Combine translation and rotation + this.velocity.x = velocityX + rotX; + this.velocity.y = velocityY + rotY; + + // Calculate speed and angle + this.speed = this.velocity.magnitude(); + this.angle = this.velocity.angle(); } }