diff --git a/src/main/kotlin/org/team5499/frc2019/Constants.kt b/src/main/kotlin/org/team5499/frc2019/Constants.kt
index 5554293..28be2e3 100644
--- a/src/main/kotlin/org/team5499/frc2019/Constants.kt
+++ b/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")
diff --git a/src/main/kotlin/org/team5499/frc2019/Robot.kt b/src/main/kotlin/org/team5499/frc2019/Robot.kt
index d3fdd70..a7b3636 100644
--- a/src/main/kotlin/org/team5499/frc2019/Robot.kt
+++ b/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}")
+    }
 }
diff --git a/src/main/kotlin/org/team5499/frc2019/auto/Routines.kt b/src/main/kotlin/org/team5499/frc2019/auto/Routines.kt
index f969589..5c85880 100644
--- a/src/main/kotlin/org/team5499/frc2019/auto/Routines.kt
+++ b/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(
diff --git a/src/main/kotlin/org/team5499/frc2019/auto/actions/VisionGoalAction.kt b/src/main/kotlin/org/team5499/frc2019/auto/actions/VisionGoalAction.kt
new file mode 100644
index 0000000..709d889
--- /dev/null
+++ b/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)
+    }
+}
diff --git a/src/main/kotlin/org/team5499/frc2019/controllers/AutoController.kt b/src/main/kotlin/org/team5499/frc2019/controllers/AutoController.kt
index b403271..50d4f01 100644
--- a/src/main/kotlin/org/team5499/frc2019/controllers/AutoController.kt
+++ b/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()
diff --git a/src/main/kotlin/org/team5499/frc2019/controllers/TeleopController.kt b/src/main/kotlin/org/team5499/frc2019/controllers/TeleopController.kt
index f4c25ae..0d748da 100644
--- a/src/main/kotlin/org/team5499/frc2019/controllers/TeleopController.kt
+++ b/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
     }
 }
diff --git a/src/main/kotlin/org/team5499/frc2019/input/IDriverControls.kt b/src/main/kotlin/org/team5499/frc2019/input/IDriverControls.kt
index f68c105..0949e85 100644
--- a/src/main/kotlin/org/team5499/frc2019/input/IDriverControls.kt
+++ b/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()}"
     }
 }
diff --git a/src/main/kotlin/org/team5499/frc2019/input/WheelDriver.kt b/src/main/kotlin/org/team5499/frc2019/input/WheelDriver.kt
index b22907c..0c57c59 100644
--- a/src/main/kotlin/org/team5499/frc2019/input/WheelDriver.kt
+++ b/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
 }
diff --git a/src/main/kotlin/org/team5499/frc2019/input/XboxDriver.kt b/src/main/kotlin/org/team5499/frc2019/input/XboxDriver.kt
index 6bc015f..7db9210 100644
--- a/src/main/kotlin/org/team5499/frc2019/input/XboxDriver.kt
+++ b/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)
 }
diff --git a/src/main/kotlin/org/team5499/frc2019/subsystems/HatchMech.kt b/src/main/kotlin/org/team5499/frc2019/subsystems/HatchMech.kt
index bc195e5..e8782b8 100644
--- a/src/main/kotlin/org/team5499/frc2019/subsystems/HatchMech.kt
+++ b/src/main/kotlin/org/team5499/frc2019/subsystems/HatchMech.kt
@@ -60,7 +60,7 @@ public class HatchMech(talon: LazyTalonSRX) : Subsystem() {
         Dashboard.addInlineListener("Constants.Hatch.POSITION_OFFSET") {
             _: String, value: Int? ->
             if (value != null) {
-                mPositionOffset = value!!
+                mPositionOffset = value
             }
         }
     }
@@ -76,7 +76,7 @@ public class HatchMech(talon: LazyTalonSRX) : Subsystem() {
     }
 
     public override fun update() {
-        println("hatchmech pos: ${mTalon.getSelectedSensorPosition(0)}")
+        // println("hatchmech pos: ${mTalon.getSelectedSensorPosition(0)}")
     }
 
     public override fun stop() {
diff --git a/src/main/kotlin/org/team5499/frc2019/subsystems/Vision.kt b/src/main/kotlin/org/team5499/frc2019/subsystems/Vision.kt
index a0c237f..679c2e3 100644
--- a/src/main/kotlin/org/team5499/frc2019/subsystems/Vision.kt
+++ b/src/main/kotlin/org/team5499/frc2019/subsystems/Vision.kt
@@ -1,15 +1,123 @@
 package org.team5499.frc2019.subsystems
 
+import org.team5499.frc2019.Constants
+
 import org.team5499.monkeyLib.Subsystem
 
+import edu.wpi.first.networktables.NetworkTableInstance
+import edu.wpi.first.networktables.NetworkTable
+
+import kotlin.math.tan
+import kotlin.math.hypot
+
 public class Vision : Subsystem() {
 
+    @Suppress("MagicNumber")
+    public enum class LEDState(val value: Int) { PIPELINE(0), OFF(1), BLINK(2), ON(3) }
+
+    @Suppress("MagicNumber")
+    public enum class VisionMode(val value: Int) { DRIVER(9), VISION(0) } // pipeline
+
+    private val mTable: NetworkTable
+
+    private var mBlinking: Boolean
+    private var mBlinkDuration: Double
+
+    public var ledState: LEDState = LEDState.OFF
+        set(value) {
+            mTable.getEntry("ledMode").setNumber(value.value)
+            field = value
+        }
+    public var visionMode: VisionMode = VisionMode.VISION
+        set(value) {
+            mTable.getEntry("pipeline").setNumber(value.value)
+            field = value
+        }
+    public val hasValidTarget: Boolean
+        get() {
+            return if (
+                mTable.getEntry("tv").getDouble(0.0) == 1.0
+            ) true
+            else false
+        }
+    public val targetXOffset: Double
+        get() {
+            return mTable.getEntry("tx").getDouble(0.0)
+        }
+    public val targetYOffset: Double
+        get() {
+            return mTable.getEntry("ty").getDouble(0.0)
+        }
+    public val targetSkew: Double
+        get() {
+            return mTable.getEntry("ts").getDouble(0.0)
+        }
+    public val targetArea: Double
+        get() {
+            return mTable.getEntry("ta").getDouble(0.0)
+        }
+
+    // use this distance method
+    public val distanceToTarget: Double
+        get() {
+            val array = mTable.getEntry("camtran").getDoubleArray(arrayOf<Double>(0.0, 0.0, 0.0, 0.0, 0.0, 0.0))
+            val x = array[0]
+            val z = array[2]
+            val distance = hypot(x, z)
+            return distance
+        }
+
+    // http://docs.limelightvision.io/en/latest/cs_estimating_distance.html
+    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 {
+        mTable = NetworkTableInstance.getDefault().getTable("limelight")
+        mBlinking = false
+        mBlinkDuration = 0.0
+        initialize()
+    }
+
+    // need to wait for limelight to boot
+    public fun initialize() {
+        mTable.getEntry("camMode").setNumber(0)
+        mTable.getEntry("stream").setNumber(0)
+        mTable.getEntry("ledMode").setNumber(0)
+        ledState = LEDState.OFF
+        visionMode = VisionMode.VISION
+    }
+
+    public fun flashForSeconds(seconds: Double) {
+        mBlinking = true
+        mBlinkDuration = seconds
+        timer.stop()
+        timer.reset()
+        timer.start()
+    }
+
     public override fun update() {
+        if (timer.get() >= mBlinkDuration) {
+            mBlinking = false
+            mBlinkDuration = 0.0
+        } else if (mBlinking) {
+            ledState = LEDState.BLINK
+        }
     }
 
     public override fun stop() {
+        mBlinking = false
+        mBlinkDuration = 0.0
     }
 
     public override fun reset() {
+        stop()
     }
 }