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 index dc594cba301e..5876fc7e9421 100644 --- 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,