Better mode changing controls

src/main/kotlin/frc/robot/Constants.kt

@@ -117,7 +117,12 @@ class Constants {
         val kTrackwidthMeters = 0.558
         val kDriveKinematics = DifferentialDriveKinematics(kTrackwidthMeters)
 
-        val ballHeightForAutoIntake = 100.0
+        val kballHeightForAutoIntake = 0.33 // a third of the screen
+        val kBallAreaThreshold = 0.1
+
+        val kDriveToggleHoldTime = 5 // seconds to hold button to change drive mode
+        val kDriveModeChangeRumbleTime = 1
+        val kDriveModeChangingOn = true // turn this off to prevent changing
 
         val constants = mutableMapOf(
             "DrivetrainPID_P" to 0.035,

src/main/kotlin/frc/robot/RobotContainer.kt

@@ -77,7 +77,10 @@ class RobotContainer {
     /*** --- commands --- ***/
     //drive by a joystick (controller1)
     val XboxDriveCommand = XboxDrive(drivetrain, controller1)
-    val XboxDriveSpecial = XboxDrive(drivetrain, controller1)
+    val XboxDriveSpecial = XboxDriveSpecial(drivetrain, controller1)
+    val driveModeToggle = DriveModeToggleSubsystem(drivetrain, controller1,
+            listOf(XboxDriveCommand, XboxDriveSpecial), 0
+    )
 
     /** -- 0 point autos -- **/
     val noAuto = DriveDoubleSupplier(drivetrain, { 0.0 }, { 0.0 })
@@ -227,7 +230,11 @@ class RobotContainer {
         /* setup default commands */
         drivetrain.defaultCommand = XboxDriveCommand
         //drivetrain.defaultCommand = XboxDriveSpecial
-
+        driveModeToggle.defaultCommand = ChangeDriveMode(driveModeToggle) {
+            Constants.kDriveModeChangingOn && controller1.aButton
+        }
+
+
         /* default gate - run forward on X, backwards on A
          * If left bumper held, run until a ball is seen by the sensor
          */
@@ -268,23 +275,32 @@ class RobotContainer {
 
         lights.defaultCommand = setAlliance
 
-        /* toggle vision mode when start is pressed on controller0 */
-        visionToggle.defaultCommand = VisionModeChange(visionToggle, {
-            val visionMode = {if (controller0.getStartButtonPressed()) {
-                when (visionToggle.visionMode) {
-                    VisionModes.BALL -> VisionModes.HIGHGOAL
-                    VisionModes.HIGHGOAL -> VisionModes.BALL
+        /* toggle vision mode with D-Pad (pov) and toggle
+         auto intake with start and back on controller0.
+          controller1 can also change vision mode */
+        visionToggle.defaultCommand = VisionModeChange(visionToggle) {
+            val visionMode = {
+                when (controller1.pov) {
+                    90 -> VisionModes.HIGHGOAL // right
+                    270 -> VisionModes.BALL // left
+                    else -> when (controller0.pov) {
+                        90 -> VisionModes.HIGHGOAL // right
+                        270 -> VisionModes.BALL // left
+                        else -> visionToggle.visionMode
+                    }
                 }
-            } else {
-                visionToggle.visionMode
-            } }
+            }
 
-            val autoIntakeOn = {if (controller0.getBackButtonPressed()) {
-                !visionToggle.visionIntakeOn
-            } else visionToggle.visionIntakeOn }
+            val autoIntakeOn = {
+                when {
+                    controller0.startButtonPressed -> true
+                    controller0.backButtonPressed -> false
+                    else -> visionToggle.visionIntakeOn
+                }
+            }
 
             Pair(visionMode(), autoIntakeOn())
-        })
+        }
 
         /* set options for autonomous */
         m_autoCommandChooser.setDefaultOption("Power Port Vision Autonomous", visionAuto())

src/main/kotlin/frc/robot/commands/ChangeDriveMode.kt

@@ -0,0 +1,66 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2018-2019 FIRST. All Rights Reserved.                        */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package frc.robot.commands
+
+import edu.wpi.first.wpilibj2.command.CommandBase
+import frc.robot.Constants
+import frc.robot.subsystems.*
+import java.time.LocalTime
+
+class ChangeDriveMode(val driveToggle: DriveModeToggleSubsystem,
+                       val activating: () -> Boolean) : CommandBase() {
+    /**
+     * Creates a new ChangeDriveMode command.
+     *
+     * @param driveToggle The subsystem used by this command.
+     * @param activating The function to call to determine if the driver
+     *   is trying to change the mode.
+     */
+
+    init {
+        addRequirements(driveToggle)
+    }
+
+    // Called when the command is initially scheduled.
+    override fun initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    override fun execute() {
+        val now = LocalTime.now()
+
+        // handle input
+        if (activating()) {
+            if (driveToggle.holdStartTime == null) {
+                driveToggle.holdStartTime = now
+            } else if (now.minusSeconds(Constants.kDriveToggleHoldTime.toLong()).isAfter(driveToggle.holdStartTime)) {
+                driveToggle.incrementMode()
+                driveToggle.holdStartTime = null
+            }
+        } else driveToggle.holdStartTime = null
+
+        // update the drive mode
+        driveToggle.applyDriveMode()
+
+        // stop rumble if time is up
+        if (driveToggle.rumbleStartTime != null
+                && now.minusSeconds(Constants.kDriveModeChangeRumbleTime.toLong())
+                        .isAfter(driveToggle.rumbleStartTime)) {
+            driveToggle.stopRumble()
+        }
+    }
+
+    // Called once the command ends or is interrupted.
+    override fun end(interrupted: Boolean) {
+    }
+
+    // Returns true when the command should end.
+    override fun isFinished(): Boolean {
+        return false
+    }
+}

src/main/kotlin/frc/robot/commands/VisionModeChange.kt

@@ -15,9 +15,10 @@ import frc.robot.subsystems.VisionToggleSubsystem
 class VisionModeChange(val visionToggle: VisionToggleSubsystem,
                        val mode: () -> Pair<VisionModes, Boolean>) : CommandBase() {
     /**
-     * Creates a new ExampleCommand.
+     * Creates a new VisionModeChange command.
      *
-     * @param m_subsystem The subsystem used by this command.
+     * @param visionToggle The subsystem used by this command.
+     * @param mode The function to get the vision mode and automatic intake state as a pair
      */
     init {
         addRequirements(visionToggle)

src/main/kotlin/frc/robot/subsystems/DriveModeToggleSubsystem.kt

@@ -0,0 +1,51 @@
+package frc.robot.subsystems
+
+import edu.wpi.first.wpilibj.GenericHID
+import edu.wpi.first.wpilibj.XboxController
+import edu.wpi.first.wpilibj2.command.CommandBase
+import java.time.LocalTime
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import frc.robot.Constants
+import java.lang.IndexOutOfBoundsException
+import java.util.*
+import kotlin.concurrent.schedule
+
+/**
+ * Subsystem to handle changing the command used to drive the robot.
+ * This is intended to test the commands, not for use in real matches.
+ */
+
+class DriveModeToggleSubsystem(private val drivetrain: DrivetrainSubsystem,
+                               private val controller1: XboxController,
+                               private val driveModes: List<CommandBase>,
+                               var currentMode: Int) : SubsystemBase() {
+
+    var holdStartTime: LocalTime? = null
+    var rumbleStartTime: LocalTime? = null
+
+    override fun periodic() {
+    }
+
+    fun incrementMode() {
+        currentMode = (currentMode + 1) % driveModes.size
+
+        // rumble the controller
+        controller1.setRumble(GenericHID.RumbleType.kLeftRumble, 1.0)
+        controller1.setRumble(GenericHID.RumbleType.kRightRumble, 1.0)
+        rumbleStartTime = LocalTime.now()
+    }
+
+    fun stopRumble() {
+        rumbleStartTime = null
+        controller1.setRumble(GenericHID.RumbleType.kLeftRumble, 0.0)
+        controller1.setRumble(GenericHID.RumbleType.kRightRumble, 0.0)
+    }
+
+    fun applyDriveMode() {
+        drivetrain.defaultCommand = try {
+            driveModes[currentMode]
+        } catch (e: IndexOutOfBoundsException) { // this shouldn't happen, but just in case
+            driveModes[0]
+        }
+    }
+}

src/main/kotlin/frc/robot/subsystems/IntakeSubsystem.kt

@@ -1,5 +1,6 @@
 package frc.robot.subsystems
 
+import java.time.LocalTime
 import edu.wpi.first.networktables.NetworkTable
 import edu.wpi.first.networktables.NetworkTableInstance
 import edu.wpi.first.wpilibj.SpeedController
@@ -29,6 +30,10 @@ class IntakeSubsystem(val motor0: SpeedController,
 
     var internalSpeed = 0.0
 
+    var intakeShutoffDelayStartTime: LocalTime? = null
+    val autoShutoffDelay: Long = 1 // seconds after not seeing a ball to keep running automatic intake
+    var runningIntakeAutomatically = false
+
     companion object {
         val ntInst = NetworkTableInstance.getDefault()
         val table = ntInst.getTable("ball-vision")
@@ -72,8 +77,33 @@ class IntakeSubsystem(val motor0: SpeedController,
     }
 
     fun getRunningAutomatic(): Boolean {
-        return (visionToggle.visionIntakeOn
+        val runningBefore = runningIntakeAutomatically
+        val shouldRunIntake = (visionToggle.visionIntakeOn
                 && ballFound.getBoolean(false)
-                && ballHeight.getDouble(0.0) >= Constants.ballHeightForAutoIntake)
+                && ballHeight.getDouble(0.0) >= Constants.kballHeightForAutoIntake)
+
+        // unless the system is turned off, wait for some time before turning off the intake
+        if (!visionToggle.visionIntakeOn) runningIntakeAutomatically = false
+        else if (shouldRunIntake) {
+            // cancel any delays and start the intake
+            intakeShutoffDelayStartTime = null
+            runningIntakeAutomatically = true
+        } else {
+            val now = LocalTime.now()
+
+            // handle losing sight of the ball
+            if (runningBefore && !shouldRunIntake) {
+                if (intakeShutoffDelayStartTime == null) {
+                    // start delay if we haven't already
+                    intakeShutoffDelayStartTime = now
+                } else if (now.minusSeconds(autoShutoffDelay).isAfter(intakeShutoffDelayStartTime)) {
+                    // the delay has passed and we don't see the ball, turn off the automatic intake
+                    intakeShutoffDelayStartTime = null
+                    runningIntakeAutomatically = false
+                }
+            }
+        }
+
+        return runningIntakeAutomatically
     }
 }