add drive mode changing for testing purposes

Author: henrymwestfall

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

@@ -120,6 +120,10 @@ class Constants {
         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,
             "DrivetrainPID_I" to 0.0,

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
          */

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

@@ -0,0 +1,63 @@
+/*----------------------------------------------------------------------------*/
+/* 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 ExampleCommand.
+     *
+     * @param m_subsystem The subsystem used by this command.
+     */
+    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/subsystems/DriveModeToggleSubsystem.kt

@@ -0,0 +1,46 @@
+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
+
+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]
+        }
+    }
+}