Adding Vision to robot

src/main/kotlin/org/team5499/frc2019/Constants.kt

@@ -175,6 +175,29 @@ public object Constants {
         public var HOLD_POSITION by DashboardVar(170)
     }
 
+    object Vision {
+        public var CAMERA_HEIGHT by DashboardVar(33.0)
+        public var CAMERA_VERTICAL_ANGLE by DashboardVar(0.0)
+        public var CAMERA_HORIZONTAL_ANGLE by DashboardVar(7.0)
+        public var HATCH_TARGET_HEIGHT by DashboardVar(29.0)
+        public var BALL_TARGET_HEIGHT by DashboardVar(36.0)
+
+        public var TARGET_DISTANCE by DashboardVar(24.0) // inches
+
+        public var ACCEPTABLE_ANGLE_ERROR by DashboardVar(3.0) // degrees(?)
+        public var ACCEPTABLE_DISTANCE_ERROR by DashboardVar(2.0) // inches (?)
+
+        public var ANGLE_KP by DashboardVar(1.0)
+        public var ANGLE_KI by DashboardVar(0.0)
+        public var ANGLE_KD by DashboardVar(0.1)
+        public var ANGLE_KF by DashboardVar(0.0)
+
+        public var DISTANCE_KP by DashboardVar(0.35)
+        public var DISTANCE_KI by DashboardVar(0.0)
+        public var DISTANCE_KD by DashboardVar(0.0)
+        public var DISTANCE_KF by DashboardVar(0.0)
+    }
+
     object Auto {
         fun initProps() {
             println("init Auto")

src/main/kotlin/org/team5499/frc2019/Robot.kt

@@ -15,6 +15,8 @@ import org.team5499.frc2019.subsystems.Drivetrain
 import org.team5499.frc2019.subsystems.Lift
 import org.team5499.frc2019.subsystems.Intake
 import org.team5499.frc2019.subsystems.Vision
+import org.team5499.frc2019.subsystems.Vision.LEDState
+import org.team5499.frc2019.subsystems.Vision.VisionMode
 import org.team5499.frc2019.subsystems.HatchMech
 import org.team5499.frc2019.controllers.SandstormController
 import org.team5499.frc2019.controllers.TeleopController
@@ -174,6 +176,8 @@ class Robot : TimedRobot(Constants.ROBOT_UPDATE_PERIOD) {
 
     override fun robotInit() {
         Logger.warn("Robot initializing" as Any)
+        mVision.ledState = Vision.LEDState.OFF
+        mVision.visionMode = Vision.VisionMode.VISION
     }
 
     override fun robotPeriodic() {
@@ -196,11 +200,13 @@ class Robot : TimedRobot(Constants.ROBOT_UPDATE_PERIOD) {
     override fun disabledInit() {
         Logger.warn("Robot disabling" as Any)
         mLift.zeroed = false
+        mVision.initialize()
+        mVision.ledState = Vision.LEDState.OFF
+        mVision.visionMode = Vision.VisionMode.VISION
         mSubsystemsManager.resetAll()
     }
 
-    override fun disabledPeriodic() {
-    }
+    override fun disabledPeriodic() {}
 
     override fun autonomousInit() {
         Logger.warn("Robot going autonomous" as Any)
@@ -224,4 +230,13 @@ class Robot : TimedRobot(Constants.ROBOT_UPDATE_PERIOD) {
         mTeleopController.update()
         mSubsystemsManager.updateAll()
     }
+
+    override fun testInit() {
+        mVision.ledState = LEDState.ON
+        mVision.visionMode = VisionMode.VISION
+    }
+
+    override fun testPeriodic() {
+        println("distance to target ${mVision.distanceToTarget}")
+    }
 }

src/main/kotlin/org/team5499/frc2019/auto/Routines.kt

@@ -17,6 +17,8 @@ import org.team5499.frc2019.auto.actions.LiftAction
 import org.team5499.frc2019.auto.actions.HatchMechAction
 import org.team5499.frc2019.auto.actions.WaitForLiftZeroAction
 import org.team5499.frc2019.auto.actions.CrossedXBoundaryAction
+import org.team5499.frc2019.auto.actions.VisionGoalAction
+import org.team5499.frc2019.auto.actions.VisionGoalAction.VisionGoal
 
 import java.util.HashMap
 
@@ -182,13 +184,17 @@ public class Routines(paths: Paths, subsystems: SubsystemsManager) {
     private fun createTuning() = Routine(
         "Tuning",
         Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0)),
-        PathAction(15.0, mPaths.tuning, mSubsystems.drivetrain)
+        PathAction(120.0, mPaths.tuning, mSubsystems.drivetrain)
     )
 
     private fun createTest() = Routine(
-        "Test",
-        Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0.0)),
-        DriveStraightAction(10.0, 12.0, mSubsystems.drivetrain)
+        "test",
+        Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0)),
+        WaitForLiftZeroAction(mSubsystems.lift),
+        LiftAction(LiftHeight.HATCH_LOW, mSubsystems.lift),
+        HatchMechAction(HatchMechPosition.HOLD, mSubsystems.hatchMech),
+        VisionGoalAction(120.0, VisionGoal.HATCH_TARGET, mSubsystems.vision, mSubsystems.drivetrain),
+        HatchMechAction(HatchMechPosition.DEPLOYED, mSubsystems.hatchMech)
     )
 
     private fun createNothing() = Routine(

src/main/kotlin/org/team5499/frc2019/auto/actions/VisionGoalAction.kt

@@ -0,0 +1,95 @@
+package org.team5499.frc2019.auto.actions
+
+import org.team5499.monkeyLib.auto.Action
+import org.team5499.monkeyLib.math.pid.PIDF
+
+import org.team5499.frc2019.subsystems.Vision
+import org.team5499.frc2019.subsystems.Drivetrain
+import org.team5499.frc2019.Constants
+
+public class VisionGoalAction(
+    timeout: Double,
+    val goal: VisionGoal,
+    val vision: Vision,
+    val drivetrain: Drivetrain
+) : Action(timeout) {
+
+    public enum class VisionGoal { HATCH_TARGET, BALL_TARGET }
+
+    private val mAnglePID: PIDF
+    private val mDistancePID: PIDF
+
+    init {
+        mAnglePID = PIDF(
+            Constants.Vision.ANGLE_KP,
+            Constants.Vision.ANGLE_KI,
+            Constants.Vision.ANGLE_KI,
+            Constants.Vision.ANGLE_KD,
+            false
+        )
+        mDistancePID = PIDF(
+            Constants.Vision.DISTANCE_KP,
+            Constants.Vision.DISTANCE_KI,
+            Constants.Vision.DISTANCE_KD,
+            Constants.Vision.DISTANCE_KF,
+            false
+        )
+    }
+
+    public override fun start() {
+
+        // reset pid for dashboard
+        mAnglePID.reset()
+        mDistancePID.reset()
+
+        mAnglePID.kP = Constants.Vision.ANGLE_KP
+        mAnglePID.kI = Constants.Vision.ANGLE_KI
+        mAnglePID.kD = Constants.Vision.ANGLE_KD
+        mAnglePID.kF = Constants.Vision.ANGLE_KF
+
+        mDistancePID.kP = Constants.Vision.DISTANCE_KP
+        mDistancePID.kI = Constants.Vision.DISTANCE_KI
+        mDistancePID.kD = Constants.Vision.DISTANCE_KD
+        mDistancePID.kF = Constants.Vision.DISTANCE_KF
+
+        mAnglePID.setpoint = -Constants.Vision.CAMERA_HORIZONTAL_ANGLE
+        mDistancePID.setpoint = Constants.Vision.TARGET_DISTANCE
+
+        // turn on leds
+        // vision.ledState = Vision.LEDState.ON
+    }
+
+    public override fun update() {
+
+        if (!vision.hasValidTarget) {
+            drivetrain.setVelocity(0.0, 0.0)
+        } else {
+            mAnglePID.processVariable = -vision.targetXOffset
+            var steer = mAnglePID.calculate()
+
+            // mDistancePID.processVariable = when (goal) {
+            //     VisionGoal.BALL_TARGET -> vision.distanceToBallTarget
+            //     VisionGoal.HATCH_TARGET -> vision.distanceToHatchTarget
+            // }
+            mDistancePID.processVariable = -vision.distanceToTarget
+            val drive = mDistancePID.calculate()
+
+            val left = drive + steer
+            val right = drive - steer
+
+            drivetrain.setVelocity(left, right)
+        }
+    }
+
+    public override fun next(): Boolean {
+        return super.timedOut() || (
+            vision.distanceToTarget < Constants.Vision.TARGET_DISTANCE // &&
+            // abs(mAnglePID.error) < Constants.Vision.ACCEPTABLE_ANGLE_ERROR &&
+            // abs(mDistancePID.error) < Constants.Vision.ACCEPTABLE_DISTANCE_ERROR
+        )
+    }
+
+    public override fun finish() {
+        drivetrain.setVelocity(0.0, 0.0)
+    }
+}

src/main/kotlin/org/team5499/frc2019/controllers/AutoController.kt

@@ -7,6 +7,8 @@ import org.team5499.monkeyLib.math.geometry.Rotation2d
 
 import org.team5499.frc2019.subsystems.SubsystemsManager
 import org.team5499.frc2019.auto.Routines
+import org.team5499.frc2019.subsystems.Vision.LEDState
+import org.team5499.frc2019.subsystems.Vision.VisionMode
 
 import org.team5499.dashboard.StringChooser
 
@@ -48,6 +50,8 @@ public class AutoController(subsystems: SubsystemsManager, routines: Routines) :
         currentRoutine = if (routine == null) mRoutines.baseline else routine
         // currentRoutine = mRoutines.rocketRight
         mSubsystems.drivetrain.brakeMode = true
+        mSubsystems.vision.ledState = LEDState.ON
+        mSubsystems.vision.visionMode = VisionMode.VISION
         mSubsystems.drivetrain.heading = Rotation2d(currentRoutine.startPose.rotation)
         mSubsystems.drivetrain.setPosition(currentRoutine.startPose.translation)
         currentAction = currentRoutine.getCurrentAction()

src/main/kotlin/org/team5499/frc2019/controllers/TeleopController.kt

@@ -4,11 +4,16 @@ import org.team5499.frc2019.subsystems.SubsystemsManager
 import org.team5499.frc2019.input.ControlBoard
 import org.team5499.frc2019.subsystems.Lift.LiftHeight
 import org.team5499.frc2019.subsystems.HatchMech
+import org.team5499.frc2019.subsystems.Vision.VisionMode
+import org.team5499.frc2019.subsystems.Vision.LEDState
+
 import org.team5499.frc2019.Constants
 
 import org.team5499.monkeyLib.Controller
 import org.team5499.monkeyLib.input.DriveHelper
+import org.team5499.monkeyLib.math.pid.PIDF
 
+@Suppress("LargeClass")
 public class TeleopController(
     subsystems: SubsystemsManager,
     controlBoard: ControlBoard,
@@ -22,11 +27,15 @@ public class TeleopController(
 
     private val mDriveHelper: DriveHelper
 
+    private val mDistancePID: PIDF
+    private val mAnglePID: PIDF
+
     private var mLockHatchMech: Boolean
     private var mLockElevator: Boolean
     private var mLastLoopManualUsed: Boolean
+    private var mAutoAlign: Boolean
 
-    private var started = false
+    private var mStarted: Boolean
 
     init {
         mSubsystems = subsystems
@@ -35,26 +44,94 @@ public class TeleopController(
         mLockHatchMech = false
         mLockElevator = false
         mLastLoopManualUsed = false
+        mStarted = false
+        mAutoAlign = false
+
+        mAnglePID = PIDF(
+            Constants.Vision.ANGLE_KP,
+            Constants.Vision.ANGLE_KI,
+            Constants.Vision.ANGLE_KI,
+            Constants.Vision.ANGLE_KD,
+            false
+        )
+        mDistancePID = PIDF(
+            Constants.Vision.DISTANCE_KP,
+            Constants.Vision.DISTANCE_KI,
+            Constants.Vision.DISTANCE_KD,
+            Constants.Vision.DISTANCE_KF,
+            false
+        )
     }
 
     public override fun start() {
-        if (!started) {
+        if (!mStarted) {
             mSubsystems.drivetrain.brakeMode = false
             mLockHatchMech = false
             mLockElevator = false
-            started = true
+            mStarted = true
+            mSubsystems.vision.ledState = LEDState.OFF
+            mSubsystems.vision.visionMode = VisionMode.DRIVER
+
+            mAnglePID.reset()
+            mDistancePID.reset()
+
+            mAnglePID.kP = Constants.Vision.ANGLE_KP
+            mAnglePID.kI = Constants.Vision.ANGLE_KI
+            mAnglePID.kD = Constants.Vision.ANGLE_KD
+            mAnglePID.kF = Constants.Vision.ANGLE_KF
+
+            mDistancePID.kP = Constants.Vision.DISTANCE_KP
+            mDistancePID.kI = Constants.Vision.DISTANCE_KI
+            mDistancePID.kD = Constants.Vision.DISTANCE_KD
+            mDistancePID.kF = Constants.Vision.DISTANCE_KF
         }
     }
 
     @Suppress("ComplexMethod")
     public override fun update() {
-        val driveSignal = mDriveHelper.calculateOutput(
-            -mControlBoard.driverControls.getThrottle(),
-            mControlBoard.driverControls.getTurn(),
-            mControlBoard.driverControls.getQuickTurn(),
-            mControlBoard.driverControls.getCreep()
-        )
-        mSubsystems.drivetrain.setPercent(driveSignal.left, driveSignal.right)
+        val isAutoAlign = mControlBoard.driverControls.getAutoAlign()
+        if (!mAutoAlign && isAutoAlign) {
+            mAutoAlign = true
+            mDistancePID.reset()
+            mAnglePID.reset()
+            mDistancePID.setpoint = Constants.Vision.TARGET_DISTANCE
+            mAnglePID.setpoint = -Constants.Vision.CAMERA_HORIZONTAL_ANGLE
+            mSubsystems.vision.ledState = LEDState.ON
+            mSubsystems.vision.visionMode = VisionMode.VISION
+        } else if (mAutoAlign && !isAutoAlign) {
+            mAutoAlign = false
+            mSubsystems.vision.ledState = LEDState.OFF
+            mSubsystems.vision.visionMode = VisionMode.DRIVER
+        }
+
+        if (!mAutoAlign) {
+            // driver can control
+            val driveSignal = mDriveHelper.calculateOutput(
+                -mControlBoard.driverControls.getThrottle(),
+                mControlBoard.driverControls.getTurn(),
+                mControlBoard.driverControls.getQuickTurn(),
+                mControlBoard.driverControls.getCreep()
+            )
+            mSubsystems.drivetrain.setPercent(driveSignal.left, driveSignal.right)
+        } else {
+            // vision system can control
+            if (mSubsystems.vision.hasValidTarget) {
+                // mSubsystems.vision.ledState = LEDState.ON
+
+                mAnglePID.processVariable = -mSubsystems.vision.targetXOffset
+                mDistancePID.processVariable = -mSubsystems.vision.distanceToTarget
+
+                val drive = mDistancePID.calculate()
+                val steer = mAnglePID.calculate()
+                val left = drive + steer
+                val right = drive - steer
+                mSubsystems.drivetrain.setVelocity(left, right)
+            } else {
+                // if not target, stop and blink
+                mSubsystems.drivetrain.setVelocity(0.0, 0.0)
+                // mSubsystems.vision.ledState = LEDState.BLINK
+            }
+        }
 
         if (mControlBoard.codriverControls.getExaust()) {
             mSubsystems.intake.outtake()
@@ -135,6 +212,7 @@ public class TeleopController(
     public override fun reset() {
         mLockHatchMech = false
         mLockElevator = false
-        started = false
+        mStarted = false
+        mAutoAlign = false
     }
 }

src/main/kotlin/org/team5499/frc2019/input/IDriverControls.kt

@@ -21,8 +21,10 @@ public interface IDriverControls {
 
     public fun getStow(): Boolean
 
+    public fun getAutoAlign(): Boolean
+
     public fun getState(): String {
         @Suppress("MaxLineLength")
-        return "${getThrottle()} : ${getTurn()} : ${getLeft()} : ${getRight()} : ${getQuickTurn()} : ${getCreep()} : ${getExitAuto()} : ${getStow()}"
+        return "${getThrottle()} : ${getTurn()} : ${getLeft()} : ${getRight()} : ${getQuickTurn()} : ${getCreep()} : ${getExitAuto()} : ${getStow()} : ${getAutoAlign()}"
     }
 }

src/main/kotlin/org/team5499/frc2019/input/WheelDriver.kt

@@ -34,4 +34,6 @@ public class WheelDriver(wheel: Joystick, throttle: Joystick) : IDriverControls
     public override fun getExitAuto() = mWheel.getRawButton(1) // paddle
 
     public override fun getStow() = false // change this later
+
+    public override fun getAutoAlign() = false
 }

src/main/kotlin/org/team5499/frc2019/input/XboxDriver.kt

@@ -27,4 +27,7 @@ public class XboxDriver(xbox: XboxController) : IDriverControls {
     public override fun getExitAuto() = mXbox.getAButtonPressed()
 
     public override fun getStow() = mXbox.getBButtonPressed()
+
+    @Suppress("MagicNumber")
+    public override fun getAutoAlign() = (mXbox.getTriggerAxis(Hand.kLeft) > 0.15)
 }