Fix vector calculation

lasarobotics · Sep 18, 2024 · ae1b558 · ae1b558
1 parent 66d8012
commit ae1b558
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java b/src/main/java/org/lasarobotics/drive/MAXSwerveModule.java
@@ -385,7 +385,9 @@ private Measure<Velocity<Distance>> getParallelVelocity(SwerveModuleState state,
     );
 
     // Calculate portion of inertial velocity that is in direction of desired
-    var parallelModuleVelocity = moduleDesiredVelocityVector.scalarMultiply(moduleInertialVelocityVector.dotProduct(moduleDesiredVelocityVector));
+    var parallelModuleVelocity = moduleDesiredVelocityVector.scalarMultiply(
+      moduleInertialVelocityVector.dotProduct(moduleDesiredVelocityVector.normalize())
+    );
 
     // Return velocity
     return Units.MetersPerSecond.of(parallelModuleVelocity.getNorm());