fixes (#95)

Team3256 · Mar 8, 2022 · 0cbdfc5 · 0cbdfc5
1 parent df52761
commit 0cbdfc5
Showing 1 changed file with 2 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/helper/SmoothVelocity.java b/src/main/java/frc/robot/helper/SmoothVelocity.java
@@ -1,16 +1,8 @@
 package frc.robot.helper;
 
 public class SmoothVelocity {
-
-    public void SmoothVelocity() {
-    }
-
     public static double smoothVelocity(double currentVelocity, double targetVelocity, double acceleration, double time) {
-        double delta = targetVelocity - currentVelocity;
-        if (Math.abs(delta) < acceleration) {
-            return targetVelocity;
-        } else {
-            return currentVelocity + Math.signum(delta) * acceleration * time;
-        }
+        double currentAcceleration = (targetVelocity - currentVelocity)/time;
+        return currentVelocity + Math.signum(currentAcceleration) * Math.min(Math.abs(currentAcceleration), acceleration) * time;
     }
 }