BUG LOCATED

GearUp12499-org · Jan 9, 2024 · 3b08d84 · 3b08d84
1 parent 1f7316e
commit 3b08d84
Showing 1 changed file with 18 additions and 0 deletions.
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/MotorPowers.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utilities/MotorPowers.kt
@@ -30,6 +30,9 @@ data class MotorPowers(
         )
     }
 
+    /**
+     * Normalize powers. Warning: may PULL SMALL NUMBERS UP to 1.
+     */
     @JvmOverloads
     fun normalize(factor: Double = 1.0): MotorPowers {
         val den = maxOf(
@@ -41,6 +44,21 @@ data class MotorPowers(
         return (this / den) * factor
     }
 
+    /**
+     * Normalize powers if they are too large.
+     */
+    @JvmOverloads
+    fun normalNoStretch(max: Double = 1.0): MotorPowers {
+        val den = maxOf(
+            abs(frontLeft),
+            abs(frontRight),
+            abs(backLeft),
+            abs(backRight),
+            max
+        )
+        return (this / den) * max
+    }
+
     operator fun div(den: Double): MotorPowers {
         return MotorPowers(
             frontLeft / den,