Adding Vision to robot

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

@@ -162,10 +162,6 @@ public object Constants {
     object Hatch {
         public const val TALON_PORT = 10
 
-        // public const val HATCH_KP = 2.0
-        // public const val HATCH_KI = 0.0
-        // public const val HATCH_KD = 1.0
-
         public var KP by DashboardVar(3.5)
         public var KI by DashboardVar(0.0)
         public var KD by DashboardVar(0.5)
@@ -179,6 +175,28 @@ public object Constants {
         public var HOLD_POSITION by DashboardVar(190)
     }
 
+    object Vision {
+        public var CAMERA_HEIGHT by DashboardVar(24.0)
+        public var CAMERA_VERTICAL_ANGLE by DashboardVar(0.0)
+        public var HATCH_TARGET_HEIGHT by DashboardVar(20.0)
+        public var BALL_TARGET_HEIGHT by DashboardVar(28.0)
+
+        public var TARGET_DISTANCE by DashboardVar(3.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.0)
+        public var ANGLE_KF by DashboardVar(0.0)
+
+        public var DISTANCE_KP by DashboardVar(1.0)
+        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

@@ -174,6 +174,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() {
@@ -184,11 +186,12 @@ class Robot : TimedRobot(Constants.ROBOT_UPDATE_PERIOD) {
     override fun disabledInit() {
         Logger.warn("Robot disabling" as Any)
         mLift.zeroed = false
+        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)
@@ -204,10 +207,8 @@ class Robot : TimedRobot(Constants.ROBOT_UPDATE_PERIOD) {
 
     override fun teleopInit() {
         Logger.warn("Robot going teleoperated" as Any)
-        // mSubsystemsManager.resetAll()
         mTeleopController.reset()
         mTeleopController.start()
-        // mLift.setZero()
     }
 
     override fun teleopPeriodic() {

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
 
@@ -139,8 +141,12 @@ public class Routines(paths: Paths, subsystems: SubsystemsManager) {
 
     private fun createTest() = Routine(
         "test",
-        Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0.0)),
-        DriveStraightAction(10.0, 12.0, mSubsystems.drivetrain)
+        Pose2d(Vector2(0, 0), Rotation2d.fromDegrees(0)),
+        WaitForLiftZeroAction(mSubsystems.lift),
+        LiftAction(LiftHeight.HATCH_LOW, mSubsystems.lift),
+        HatchMechAction(HatchMechPosition.HOLD, mSubsystems.hatchMech),
+        VisionGoalAction(20.0, VisionGoal.HATCH_TARGET, mSubsystems.vision, mSubsystems.drivetrain),
+        HatchMechAction(HatchMechPosition.DEPLOYED, mSubsystems.hatchMech)
     )
 
     public fun resetAll() {

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

@@ -0,0 +1,90 @@
+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
+
+import kotlin.math.abs
+
+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 var mAnglePID: PIDF
+    private var 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.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
+
+        // turn on leds
+        vision.ledState = Vision.LEDState.ON
+    }
+
+    public override fun update() {
+
+        if (!vision.hasValidTarget) {
+            drivetrain.setVelocity(0.0, 0.0)
+        } else {
+            mAnglePID.setpoint = 0.0
+            mAnglePID.processVariable = vision.targetXOffset
+            var steer = mAnglePID.calculate()
+
+            mDistancePID.setpoint = Constants.Vision.TARGET_DISTANCE
+            mDistancePID.processVariable = when (goal) {
+                VisionGoal.BALL_TARGET -> vision.distanceToBallTarget
+                VisionGoal.HATCH_TARGET -> vision.distanceToHatchTarget
+            }
+            val drive = mDistancePID.calculate()
+
+            val left = drive + steer
+            val right = drive - steer
+
+            drivetrain.setVelocity(left, right)
+        }
+    }
+
+    public override fun next(): Boolean {
+        return super.timedOut() || (
+            abs(mAnglePID.error) < Constants.Vision.ACCEPTABLE_ANGLE_ERROR &&
+            abs(mDistancePID.error) < Constants.Vision.ACCEPTABLE_DISTANCE_ERROR
+        )
+    }
+
+    public override fun finish() {
+        vision.ledState = Vision.LEDState.OFF
+    }
+}

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
 
@@ -31,7 +33,7 @@ public class AutoController(subsystems: SubsystemsManager, routines: Routines) :
         currentAction = null
         val tempArray = Array<String>(mRoutines.routineMap.size, { "" })
         var i = 0
-        for ((key, value) in mRoutines.routineMap) {
+        for ((key, _) in mRoutines.routineMap) {
             tempArray.set(i, key)
             i++
         }
@@ -47,6 +49,8 @@ public class AutoController(subsystems: SubsystemsManager, routines: Routines) :
         val routine = mRoutines.getRoutineWithName(mAutoSelector.selected)
         currentRoutine = if (routine == null) mRoutines.baseline else routine
         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,6 +4,8 @@ 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
@@ -37,6 +39,8 @@ public class TeleopController(
 
     public override fun start() {
         mSubsystems.drivetrain.brakeMode = false
+        mSubsystems.vision.ledState = LEDState.OFF
+        mSubsystems.vision.visionMode = VisionMode.VISION
         mLockHatchMech = false
         mLockElevator = false
     }

src/main/kotlin/org/team5499/frc2019/subsystems/HatchMech.kt

@@ -57,7 +57,7 @@ public class HatchMech(talon: LazyTalonSRX) : Subsystem() {
         Dashboard.addInlineListener("Constants.Hatch.POSITION_OFFSET") {
             _: String, value: Int? ->
             if (value != null) {
-                mPositionOffset = value!!
+                mPositionOffset = value
             }
         }
     }

src/main/kotlin/org/team5499/frc2019/subsystems/Vision.kt

@@ -1,15 +1,74 @@
 package org.team5499.frc2019.subsystems
 
+import org.team5499.frc2019.Constants
+
 import org.team5499.monkeyLib.Subsystem
 
+import edu.wpi.first.networktables.NetworkTableInstance
+
+import kotlin.math.tan
+
 public class Vision : Subsystem() {
 
-    public override fun update() {
-    }
+    @Suppress("MagicNumber")
+    public enum class LEDState(val value: Int) { PIPELINE(0), OFF(1), BLINK(2), ON(3) }
 
-    public override fun stop() {
-    }
+    @Suppress("MagicNumber")
+    public enum class VisionMode(val value: Int) { DRIVER(9), VISION(0) } // pipeline
 
-    public override fun reset() {
+    public var ledState: LEDState = LEDState.OFF
+        set(value) {
+            NetworkTableInstance.getDefault().getTable("limelight") .getEntry("ledMode").setNumber(value.value)
+            field = value
+        }
+    public var visionMode: VisionMode = VisionMode.VISION
+        set(value) {
+            NetworkTableInstance.getDefault().getTable("limelight") .getEntry("pipeline").setNumber(value.value)
+            field = value
+        }
+    public val hasValidTarget: Boolean
+        get() {
+            return if (
+                NetworkTableInstance.getDefault().getTable("limelight") .getEntry("tv").getDouble(0.0) == 1.0
+            ) true
+            else false
+        }
+    public val targetXOffset: Double
+        get() {
+            return NetworkTableInstance.getDefault().getTable("limelight") .getEntry("tx").getDouble(0.0)
+        }
+    public val targetYOffset: Double
+        get() {
+            return NetworkTableInstance.getDefault().getTable("limelight") .getEntry("ty").getDouble(0.0)
+        }
+    public val targetSkew: Double
+        get() {
+            return NetworkTableInstance.getDefault().getTable("limelight") .getEntry("ts").getDouble(0.0)
+        }
+    public val targetArea: Double
+        get() {
+            return NetworkTableInstance.getDefault().getTable("limelight") .getEntry("ta").getDouble(0.0)
+        }
+    public val distanceToHatchTarget: Double
+        get() {
+            return (Constants.Vision.HATCH_TARGET_HEIGHT - Constants.Vision.CAMERA_HEIGHT) /
+                tan(Constants.Vision.CAMERA_VERTICAL_ANGLE + targetYOffset)
+        }
+    public val distanceToBallTarget: Double
+        get() {
+            return (Constants.Vision.BALL_TARGET_HEIGHT - Constants.Vision.CAMERA_HEIGHT) /
+                tan(Constants.Vision.CAMERA_VERTICAL_ANGLE + targetYOffset)
+        }
+
+    init {
+        NetworkTableInstance.getDefault().getTable("limelight") .getEntry("camMode").setNumber(0)
+        NetworkTableInstance.getDefault().getTable("limelight") .getEntry("stream").setNumber(0)
+        NetworkTableInstance.getDefault().getTable("limelight") .getEntry("ledMode").setNumber(0)
     }
+
+    public override fun update() {}
+
+    public override fun stop() {}
+
+    public override fun reset() {}
 }