add automatic intake with ball vision

Author: henrymwestfall

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

@@ -117,6 +117,8 @@ class Constants {
         val kTrackwidthMeters = 0.558
         val kDriveKinematics = DifferentialDriveKinematics(kTrackwidthMeters)
 
+        val ballHeightForAutoIntake = 100.0
+
         val constants = mutableMapOf(
             "DrivetrainPID_P" to 0.035,
             "DrivetrainPID_I" to 0.0,

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

@@ -254,22 +254,32 @@ class RobotContainer {
 
         /* default intake - run forward on right bumper, backwards on right trigger */
         intake.defaultCommand = FixedIntakeSpeed(intake, {
-            if (controller0.getBumper(kRight) || controller1.xButton) Constants.kIntakeSpeed else (
+            if (controller0.getBumper(kRight) || controller1.xButton || intake.getRunningAutomatic()) {
+                Constants.kIntakeSpeed
+            } else (
                 if (controller0.getTriggerAxis(kRight) >= Constants.kTriggerThresh)
-                    controller0.getTriggerAxis(kRight) * -Constants.kIntakeDir else 0.0)
+                    controller0.getTriggerAxis(kRight) * -Constants.kIntakeDir
+                else 0.0)
         })
+
         lights.defaultCommand = setAlliance
 
         /* toggle vision mode when start is pressed on controller0 */
         visionToggle.defaultCommand = VisionModeChange(visionToggle, {
-            if (controller0.getStartButtonPressed()) {
+            val visionMode = {if (controller0.getStartButtonPressed()) {
                 when (visionToggle.visionMode) {
                     VisionModes.BALL -> VisionModes.HIGHGOAL
                     VisionModes.HIGHGOAL -> VisionModes.BALL
                 }
             } else {
                 visionToggle.visionMode
-            }
+            } }
+
+            val autoIntakeOn = {if (controller0.getBackButtonPressed()) {
+                !visionToggle.visionIntakeOn
+            } else visionToggle.visionIntakeOn }
+
+            Pair(visionMode(), autoIntakeOn())
         })
 
         /* set options for autonomous */

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

@@ -13,7 +13,7 @@ import frc.robot.subsystems.VisionModes
 import frc.robot.subsystems.VisionToggleSubsystem
 
 class VisionModeChange(val visionToggle: VisionToggleSubsystem,
-                       val mode: () -> VisionModes) : CommandBase() {
+                       val mode: () -> Pair<VisionModes, Boolean>) : CommandBase() {
     /**
      * Creates a new ExampleCommand.
      *
@@ -29,7 +29,9 @@ class VisionModeChange(val visionToggle: VisionToggleSubsystem,
 
     // Called every time the scheduler runs while the command is scheduled.
     override fun execute() {
-        visionToggle.visionMode = mode()
+        val currentMode = mode()
+        visionToggle.visionMode = currentMode.first
+        visionToggle.visionIntakeOn = currentMode.second
     }
 
     // Called once the command ends or is interrupted.

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

@@ -1,8 +1,11 @@
 package frc.robot.subsystems
 
+import edu.wpi.first.networktables.NetworkTable
+import edu.wpi.first.networktables.NetworkTableInstance
 import edu.wpi.first.wpilibj.SpeedController
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import frc.robot.Constants
+import frc.robot.commands.BallVision
 
 /**
  * Intake Subsystem
@@ -24,6 +27,12 @@ class IntakeSubsystem(val motor0: SpeedController, val motor1: SpeedController)
 
     var internalSpeed = 0.0
 
+    companion object {
+        val ntInst = NetworkTableInstance.getDefault()
+        val table = BallVision.ntInst.getTable("ball-vision")
+        val ballHeight = BallVision.table.getEntry("ballHeight") // the height of the ball
+    }
+
 
     private fun setTargetSpeed(target: Double) {
         internalTargetSpeed = target
@@ -58,4 +67,8 @@ class IntakeSubsystem(val motor0: SpeedController, val motor1: SpeedController)
     fun setSpeed(speed: Double) {
         setTargetSpeed(speed)
     }
+
+    fun getRunningAutomatic(): Boolean {
+        return ballHeight.getDouble(0.0) >= Constants.ballHeightForAutoIntake
+    }
 }

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

@@ -6,6 +6,7 @@ enum class VisionModes { BALL, HIGHGOAL }
 
 class VisionToggleSubsystem() : SubsystemBase() {
     var visionMode = VisionModes.BALL
+    var visionIntakeOn = true
 
     override fun periodic() {
     }