update calculateWithVelocities

Author: FriedLongJohns

src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java

@@ -195,9 +195,10 @@ public void drivePeriodic() {
         double actualSpeed = getCurrentSpeed();
         double targetVoltage = (actualSpeed >= 0 ? forwardSimpleMotorFF : backwardSimpleMotorFF)
             .calculateWithVelocities(
-                desiredSpeed, 
-                desiredSpeed+calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())
+                actualSpeed, 
+                desiredSpeed
             );//clippedAcceleration);
+        //calculateAntiGravitationalA(pitchDegSupplier.get(), rollDegSupplier.get())
 
         // Use robot characterization as a simple physical model to account for internal resistance, frcition, etc.
         // Add a PID adjustment for error correction (also "drives" the actual speed to the desired speed)