fix automatic intake

Author: henrymwestfall

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

@@ -58,21 +58,20 @@ class RobotContainer {
     val motorsLeft = SpeedControllerGroup(motorFrontLeft, motorBackLeft)
     val motorsRight = SpeedControllerGroup(motorFrontRight, motorBackRight)
 
+    val visionToggle = VisionToggleSubsystem()
 
     val gyro = AHRS()
     val leftDrivetrainEncoder = Encoder(Constants.leftDrivetrainEncoderPortA, Constants.leftDrivetrainEncoderPortB, Constants.kDrivetrainEncoderAReversed)
     val rightDrivetrainencoder = Encoder(Constants.rightDrivetrainEncoderPortA, Constants.rightDrivetrainEncoderPortB, Constants.kDrivetrainEncoderBReversed)
 
     val drivetrain = DrivetrainSubsystem(motorsLeft, motorsRight, gyro, leftDrivetrainEncoder, rightDrivetrainencoder)
     val shooter = ShooterSubsystem(CANSparkMax(Constants.kShooterPort, MotorType.kBrushless))
-    val intake = IntakeSubsystem(WPI_TalonSRX(Constants.kIntakePort), WPI_VictorSPX(Constants.kIntake2Port))
+    val intake = IntakeSubsystem(WPI_TalonSRX(Constants.kIntakePort), WPI_VictorSPX(Constants.kIntake2Port), visionToggle)
 
     val indexer = IndexerSubsystem(WPI_TalonSRX(Constants.kIndexerPort))
     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 --- ***/

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

@@ -12,7 +12,9 @@ import frc.robot.commands.BallVision
  *
  * Just a intake motor
  */
-class IntakeSubsystem(val motor0: SpeedController, val motor1: SpeedController) : SubsystemBase() {
+class IntakeSubsystem(val motor0: SpeedController,
+                      val motor1: SpeedController,
+                      val visionToggle: VisionToggleSubsystem) : SubsystemBase() {
     /* get a nice speed curve
      * do each speed transition across 20 iterations (400ms)
      */
@@ -29,9 +31,9 @@ class IntakeSubsystem(val motor0: SpeedController, val motor1: SpeedController)
 
     companion object {
         val ntInst = NetworkTableInstance.getDefault()
-        val table = BallVision.ntInst.getTable("ball-vision")
-        val ballHeight = BallVision.table.getEntry("ballHeight") // the height of the ball
-        val ballFound = BallVision.table.getEntry("ballFound")
+        val table = ntInst.getTable("ball-vision")
+        val ballHeight = table.getEntry("ballHeight") // the height of the ball
+        val ballFound = table.getEntry("ballFound")
     }
 
 
@@ -70,6 +72,8 @@ class IntakeSubsystem(val motor0: SpeedController, val motor1: SpeedController)
     }
 
     fun getRunningAutomatic(): Boolean {
-        return ballFound.getBoolean(false) && ballHeight.getDouble(0.0) >= Constants.ballHeightForAutoIntake
+        return (visionToggle.visionIntakeOn
+                && ballFound.getBoolean(false)
+                && ballHeight.getDouble(0.0) >= Constants.ballHeightForAutoIntake)
     }
 }