From 18cca15ae1eccb82d901011372e111f7bc67610a Mon Sep 17 00:00:00 2001 From: Andrew Dassonville Date: Fri, 18 May 2018 16:05:13 -0700 Subject: [PATCH] Stops driving forward without any joystick controls --- .../powerup/drivetrain/commands/TeleopDrive.kt | 12 ++++++++++-- 1 file changed, 10 insertions(+), 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 index f37e726..e1397a7 100644 --- 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 } }