add vision mode toggling

Author: henrymwestfall

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

@@ -32,6 +32,8 @@ import frc.robot.triggers.EndgameTrigger
  * periodic methods (other than the scheduler calls).  Instead, the structure of the robot
  * (including subsystems, commands, and button mappings) should be declared here.
  */
+
+
 class RobotContainer {
     val sch = CommandScheduler.getInstance()
     var m_autoCommandChooser: SendableChooser<Command> = SendableChooser()
@@ -69,6 +71,8 @@ class RobotContainer {
     val gate = GateSubsystem(WPI_TalonSRX(Constants.kGatePort), ColorSensorV3(I2C.Port.kOnboard))
     val lights = LEDSubsystem(AddressableLED(Constants.kLED0Port), 60, DriverStation.getInstance())
 
+    val visionToggle = VisionToggleSubsystem()
+
     val climber = ClimbSubsystem(WPI_TalonSRX(Constants.kClimberPort))
     val winch = WinchSubsystem(WPI_TalonSRX(Constants.kWinchPort))
     /*** --- commands --- ***/
@@ -130,6 +134,20 @@ class RobotContainer {
         )
     }
 
+    val visionBallLineup = {
+        SequentialCommandGroup(
+            BallVision(drivetrain, 0.0),
+            DriveDoubleSupplier(drivetrain, { 0.3 }, { 0.0 }).withTimeout(0.5)
+        )
+    }
+
+    val visionLineup: () -> SequentialCommandGroup = {
+        when (visionToggle.visionMode) {
+            VisionModes.HIGHGOAL -> visionHighGoalLineUp()
+            VisionModes.BALL -> visionBallLineup()
+        }
+    }
+
     val forwardShootAutoNoIntake = SequentialCommandGroup(
         ParallelCommandGroup(
             SequentialCommandGroup(
@@ -181,7 +199,7 @@ class RobotContainer {
         )
 
         JoystickButton(controller1, kBumperLeft.value).whenHeld(
-            visionHighGoalLineUp()
+            visionLineup()
         )
 
         /* Climbing, including winch */
@@ -242,6 +260,18 @@ class RobotContainer {
         })
         lights.defaultCommand = setAlliance
 
+        /* toggle vision mode when start is pressed on controller0 */
+        visionToggle.defaultCommand = VisionModeChange(visionToggle, {
+            if (controller0.getStartButtonPressed()) {
+                when (visionToggle.visionMode) {
+                    VisionModes.BALL -> VisionModes.HIGHGOAL
+                    VisionModes.HIGHGOAL -> VisionModes.BALL
+                }
+            } else {
+                visionToggle.visionMode
+            }
+        })
+
         /* set options for autonomous */
         m_autoCommandChooser.setDefaultOption("Power Port Vision Autonomous", visionAuto())
         m_autoCommandChooser.addOption("Backup 2s Autonomous", backupAuto)

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

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* 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.subsystems.ExampleSubsystem
+import frc.robot.subsystems.VisionModes
+import frc.robot.subsystems.VisionToggleSubsystem
+
+class VisionModeChange(val visionToggle: VisionToggleSubsystem,
+                       val mode: () -> VisionModes) : CommandBase() {
+    /**
+     * Creates a new ExampleCommand.
+     *
+     * @param m_subsystem The subsystem used by this command.
+     */
+    init {
+        addRequirements(visionToggle)
+    }
+
+    // Called when the command is initially scheduled.
+    override fun initialize() {
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    override fun execute() {
+        visionToggle.visionMode = mode()
+    }
+
+    // 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/GateSubsystem.kt

@@ -17,7 +17,7 @@ import kotlin.math.abs
 class GateSubsystem(val motor: SpeedController, val colorSensor: ColorSensorV3) : SubsystemBase() {
 
     override fun periodic() {
-        debugMeasurment()
+        debugMeasurement()
     }
 
     /* set a motor speed, no matter if a ball is in the gate or not */
@@ -46,7 +46,7 @@ class GateSubsystem(val motor: SpeedController, val colorSensor: ColorSensorV3)
     }
 
     /* print status of sensor + its reading */
-    fun debugMeasurment() {
+    fun debugMeasurement() {
         SmartDashboard.putBoolean("Gate Ball Sensor Status", colorSensor.getIR() != 0)
         SmartDashboard.putBoolean("Ball Present in Gate", isBallTriggered())
         SmartDashboard.putNumber("Gate Ball Sensor Reading", colorSensor.getIR().toDouble())

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

@@ -0,0 +1,12 @@
+package frc.robot.subsystems
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+
+enum class VisionModes { BALL, HIGHGOAL }
+
+class VisionToggleSubsystem() : SubsystemBase() {
+    var visionMode = VisionModes.BALL
+
+    override fun periodic() {
+    }
+}