Stops driving forward without any joystick controls

SouthEugeneRoboticsTeam · May 18, 2018 · 18cca15 · 18cca15
1 parent 3fdcb7e
commit 18cca15
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/src/main/java/org/sert2521/powerup/drivetrain/commands/TeleopDrive.kt b/src/main/java/org/sert2521/powerup/drivetrain/commands/TeleopDrive.kt
@@ -24,11 +24,19 @@ class TeleopDrive : Command() {
 
     override fun execute(): Boolean {
         val safe: Double.() -> Double = {
-            this * (GRADIENT.pow(
+            val speed = this * (GRADIENT.pow(
                     Elevator.SCALE_TARGET / (Elevator.SAFE_MAX_TARGET - Elevator.position)
             ) + MIN_SPEED)
+            if (speed < GRADIENT) {
+                if (speed > MIN_SPEED) {
+                    MIN_SPEED
+                } else speed
+            } else {
+                MIN_SPEED
+            }
         }
 
+
         when (controlMode) {
             is Control.Arcade ->
                 Drivetrain.arcade(speedScalar * -rightJoystick.y.safe(), rightJoystick.x)
@@ -53,7 +61,7 @@ class TeleopDrive : Command() {
     override fun onDestroy() = Drivetrain.stop()
 
     private companion object {
-        const val GRADIENT = 0.8
+        const val GRADIENT = 0.80
         const val MIN_SPEED = 0.17
     }
 }