diff --git a/src/main/kotlin/com/team4099/robot2021/Robot.kt b/src/main/kotlin/com/team4099/robot2021/Robot.kt
index 4be80d08..51ed0daf 100644
--- a/src/main/kotlin/com/team4099/robot2021/Robot.kt
+++ b/src/main/kotlin/com/team4099/robot2021/Robot.kt
@@ -2,7 +2,6 @@ package com.team4099.robot2021
 
 import com.team4099.lib.logging.Logger
 import com.team4099.lib.smoothDeadband
-import com.team4099.robot2021.auto.modes2021.EnemyTrenchMode
 import com.team4099.robot2021.auto.modes2021.ThreeBallMode
 import com.team4099.robot2021.commands.climber.LockClimberCommand
 import com.team4099.robot2021.commands.climber.OpenLoopClimbCommand
diff --git a/src/main/kotlin/com/team4099/robot2021/auto/modes2021/ThreeBallMode.kt b/src/main/kotlin/com/team4099/robot2021/auto/modes2021/ThreeBallMode.kt
index 370b5a48..83a81c12 100644
--- a/src/main/kotlin/com/team4099/robot2021/auto/modes2021/ThreeBallMode.kt
+++ b/src/main/kotlin/com/team4099/robot2021/auto/modes2021/ThreeBallMode.kt
@@ -3,7 +3,6 @@ package com.team4099.robot2021.auto.modes2021
 import com.team4099.robot2021.auto.PathStore
 import com.team4099.robot2021.commands.drivetrain.AutoDriveCommand
 import com.team4099.robot2021.commands.shooter.ShootAllCommand
-import com.team4099.robot2021.commands.shooter.VisionCommand
 import com.team4099.robot2021.subsystems.Drivetrain
 import com.team4099.robot2021.subsystems.Shooter
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup
diff --git a/src/main/kotlin/com/team4099/robot2021/commands/led/LEDCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/led/LEDCommand.kt
new file mode 100644
index 00000000..470a5518
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2021/commands/led/LEDCommand.kt
@@ -0,0 +1,17 @@
+package com.team4099.robot2021.commands.led
+
+import com.team4099.lib.logging.Logger
+import com.team4099.robot2021.config.Constants
+import com.team4099.robot2021.subsystems.LED
+import edu.wpi.first.wpilibj2.command.CommandBase
+
+class LEDCommand(var LEDstate: Constants.LED.Status) : CommandBase() {
+  init {
+    addRequirements(LED)
+  }
+
+  override fun initialize() {
+    LED.statusState = LEDstate
+    Logger.addEvent("LED", "LED State: $LEDstate")
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2021/commands/shooter/SpinUpCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/shooter/SpinUpCommand.kt
index 2ee9a630..acd5e092 100644
--- a/src/main/kotlin/com/team4099/robot2021/commands/shooter/SpinUpCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2021/commands/shooter/SpinUpCommand.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2021.commands.shooter
 
 import com.team4099.lib.logging.Logger
+import com.team4099.robot2021.commands.led.LEDCommand
 import com.team4099.robot2021.config.Constants
 import com.team4099.robot2021.subsystems.Shooter
 import com.team4099.robot2021.subsystems.Vision
@@ -19,6 +20,7 @@ class SpinUpCommand(
     Logger.addEvent("SpinUpCommand", "Started shooter spin-up command")
   }
   override fun execute() {
+    LEDCommand(Constants.LED.Status.SHOOTER_SPEED)
     if (withVision) {
       Shooter.targetVelocity =
           when (Vision.currentDistance) {
diff --git a/src/main/kotlin/com/team4099/robot2021/commands/shooter/VisionCommand.kt b/src/main/kotlin/com/team4099/robot2021/commands/shooter/VisionCommand.kt
index 2fd02957..8b1dca12 100644
--- a/src/main/kotlin/com/team4099/robot2021/commands/shooter/VisionCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2021/commands/shooter/VisionCommand.kt
@@ -6,6 +6,7 @@ import com.team4099.lib.units.base.meters
 import com.team4099.lib.units.derived.degrees
 import com.team4099.lib.units.derived.inDegrees
 import com.team4099.lib.units.perSecond
+import com.team4099.robot2021.commands.led.LEDCommand
 import com.team4099.robot2021.config.Constants
 import com.team4099.robot2021.subsystems.Drivetrain
 import com.team4099.robot2021.subsystems.Vision
@@ -68,6 +69,7 @@ class VisionCommand : CommandBase() {
   }
 
   override fun end(interrupted: Boolean) {
+    LEDCommand(Constants.LED.Status.VISION_LOCK)
     Drivetrain.setOpenLoop(0.degrees.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond))
   }
 }
diff --git a/src/main/kotlin/com/team4099/robot2021/config/Constants.kt b/src/main/kotlin/com/team4099/robot2021/config/Constants.kt
index c438bc13..a0fd3414 100644
--- a/src/main/kotlin/com/team4099/robot2021/config/Constants.kt
+++ b/src/main/kotlin/com/team4099/robot2021/config/Constants.kt
@@ -1,266 +1,299 @@
-package com.team4099.robot2021.config
-
-import com.team4099.lib.units.base.Length
-import com.team4099.lib.units.base.feet
-import com.team4099.lib.units.base.inches
-import com.team4099.lib.units.base.meters
-import com.team4099.lib.units.base.seconds
-import com.team4099.lib.units.derived.degrees
-import com.team4099.lib.units.derived.div
-import com.team4099.lib.units.derived.rotations
-import com.team4099.lib.units.derived.volts
-import com.team4099.lib.units.perMinute
-import com.team4099.lib.units.perSecond
-import edu.wpi.first.wpilibj.DoubleSolenoid
-
-object Constants {
-  object Universal {
-    const val CTRE_CONFIG_TIMEOUT = 0
-    const val EPSILON = 1E-9
-  }
-
-  object Tuning {
-    const val TUNING_TOGGLE_PIN = 0
-    val ROBOT_ID_PINS = 1..2
-
-    enum class RobotName {
-      COMPETITION,
-      PRACTICE,
-      MULE
-    }
-
-    val ROBOT_ID_MAP =
-        mapOf<Int, RobotName>(
-            0 to RobotName.COMPETITION, 1 to RobotName.PRACTICE, 2 to RobotName.MULE)
-  }
-
-  object Joysticks {
-    const val DRIVER_PORT = 0
-    const val SHOTGUN_PORT = 1
-    const val TECHNICIAN_PORT = 2
-
-    const val THROTTLE_DEADBAND = 0.05
-    const val TURN_DEADBAND = 0.05
-  }
-
-  object Drivetrain {
-    const val TICKS = 4096
-
-    const val FRONT_LEFT_SPEED_ID = 11
-    const val FRONT_LEFT_DIRECTION_ID = 21
-    const val FRONT_LEFT_CANCODER_ID = 2
-
-    const val FRONT_RIGHT_SPEED_ID = 12
-    const val FRONT_RIGHT_DIRECTION_ID = 22
-    const val FRONT_RIGHT_CANCODER_ID = 1
-
-    const val BACK_RIGHT_SPEED_ID = 13
-    const val BACK_RIGHT_DIRECTION_ID = 23
-    const val BACK_RIGHT_CANCODER_ID = 4
-
-    const val BACK_LEFT_SPEED_ID = 14
-    const val BACK_LEFT_DIRECTION_ID = 24
-    const val BACK_LEFT_CANCODER_ID = 3
-
-    const val WHEEL_COUNT = 4
-    val DRIVETRAIN_LENGTH = 22.173.inches
-    val DRIVETRAIN_WIDTH = 22.173.inches
-
-    val DRIVE_SETPOINT_MAX = 15.feet.perSecond
-    val TURN_SETPOINT_MAX = 360.degrees.perSecond // TODO: Make sure this value is something good
-
-    val DIRECTION_VEL_MAX = 900.degrees.perSecond
-    val DIRECTION_ACCEL_MAX = 4500.degrees.perSecond.perSecond
-
-    const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value
-
-    val MAX_AUTO_VEL = 3.meters.perSecond
-    val SLOW_AUTO_VEL = 0.66.meters.perSecond
-    val MAX_AUTO_ACCEL = 2.0.meters.perSecond.perSecond
-
-    val MAX_AUTO_ANGULAR_VEL = 90.0.degrees.perSecond
-    val MAX_AUTO_ANGULAR_ACCEL = 90.0.degrees.perSecond.perSecond
-
-    const val ABSOLUTE_GEAR_RATIO = 1.0
-    const val DRIVE_SENSOR_GEAR_RATIO = (12.0 / 21.0) * (15.0 / 45.0)
-    const val DIRECTION_SENSOR_GEAR_RATIO = (12.0 / 64.0) * (1.0 / 10.0)
-
-    val ALLOWED_ANGLE_ERROR = 1.degrees
-    const val DIRECTION_SMART_CURRENT_LIMIT = 20
-    const val DRIVE_SMART_CURRENT_LIMIT = 80
-
-    object Gains {
-      const val RAMSETE_B = 2.0
-      const val RAMSETE_ZETA = 0.7
-    }
-
-    object PID {
-      const val DIRECTION_KP = 0.00001
-      const val DIRECTION_KI = 0.0
-      const val DIRECTION_KD = 12.0
-      const val DIRECTION_KFF = 0.000078
-
-      const val DRIVE_KP = 0.000129
-      const val DRIVE_KI = 0.0
-      const val DRIVE_KD = 0.0
-      const val DRIVE_KFF = 0.0
-
-      const val AUTO_POS_KP = 0.581 / 6
-      const val AUTO_POS_KI = 0.0
-      const val AUTO_POS_KD = 0.0 // 263.0 / 6
-
-      const val DRIVE_THETA_PID_KP = 1.0
-      const val DRIVE_THETA_PID_KI = 0.0
-      const val DRIVE_THETA_PID_KD = 0.0
-      val DRIVE_THETA_PID_MAX_VEL = 0.0.meters.perSecond
-      val DRIVE_THETA_PID_MAX_ACCEL = 0.0.meters.perSecond.perSecond
-
-      val DRIVE_KS = 0.339.volts
-      val DRIVE_KV = 2.78.volts / 1.0.meters.perSecond
-      val DRIVE_KA = 0.421.volts / 1.0.meters.perSecond.perSecond
-    }
-  }
-
-  object Feeder {
-    const val FLOOR_ID = 41
-    const val VERTICAL_ID = 42
-    const val FEEDER_POWER = 1.0 // was 0.45
-    const val FAST_FEEDER_POWER = 0.75 // was 1.0
-    // add one for close and far shot
-    const val FLOOR_CURRENT_LIMIT = 15
-    const val VERTICAL_CURRENT_LIMIT = 40
-
-    const val TOP_DIO_PIN = 9
-    const val BOTTOM_DIO_PIN = 8
-    const val BEAM_BREAK_BROKEN_TIME = 0.05
-    const val BEAM_BREAK_BACKWARDS_TIME = 0.05
-    const val TAB = "Feeder"
-  }
-
-  object Intake {
-    const val INTAKE_MOTOR = 31
-    const val ARM_SOLENOID_FORWARD = 7
-    const val ARM_SOLENOID_REVERSE = 0
-    const val TAB = "Intake"
-
-    val RAMP_TIME = 1.0
-
-    enum class IntakeState(val speed: Double) {
-      IDLE(0.0),
-      IN(1.0),
-      OUT(-1.0)
-    }
-
-    enum class ArmPosition(val position: DoubleSolenoid.Value?) {
-      OUT(DoubleSolenoid.Value.kReverse),
-      IN(DoubleSolenoid.Value.kForward)
-    }
-  }
-
-  object Shooter {
-    const val SHOOTER_MOTOR_ID = 51
-    const val SHOOTER_FOLLOWER_ID = 52
-
-    const val SOLENOID_FORWARD_CHANNEL = 1
-    const val SOLENOID_REVERSE_CHANNEL = 6
-
-    val VELOCITY_TOLERANCE = 100.rotations.perMinute
-
-    const val SHOOTER_KS = 0.939 // * 2
-    const val SHOOTER_KV = 0.1 // 0.114 // * 2
-
-    const val SHOOTER_KP = 0.5 // 0.65
-    const val SHOOTER_KI = 0.0
-    const val SHOOTER_KD = 0.0
-
-    val NEAR_VELOCITY = 1700.0.rotations.perMinute
-    val LINE_VELOCITY = 2750.0.rotations.perMinute
-    val MID_VELOCITY = 2800.0.rotations.perMinute
-    val FAR_VELOCITY = 4000.0.rotations.perMinute
-    // TODO: Determine velocity needed to shoot from front of trench
-
-    val LINE_DISTANCE = 100.0.inches
-    val NEAR_DISTANCE = 130.0.inches
-    val MID_DISTANCE = 249.0.inches
-
-    val POWER_CELL_CHALLENGE_RPM = 2950.rotations.perMinute
-
-    val UNJAM_RPM = 200.rotations.perMinute
-
-    // val HOOD_THRESHOLD = 0.0.inches
-
-    const val TAB = "Shooter"
-  }
-
-  object Vision {
-    const val DRIVER_PIPELINE_ID = 1
-    const val TARGETING_PIPELINE_ID = 0
-    val TARGET_HEIGHT = 98.25.inches
-
-    val CLOSE_CAMERA_HEIGHT = 0.0.inches
-    val CLOSE_CAMERA_ANGLE = 0.0.degrees
-    val FAR_CAMERA_HEIGHT = 35.0.inches
-    val FAR_CAMERA_ANGLE = 24.0.degrees
-
-    val CAMERA_DIST_THRESHOLD = 55.0.inches
-
-    val MAX_DIST_ERROR = 0.1.inches
-    val MAX_ANGLE_ERROR = 1.0.degrees
-
-    const val MIN_TURN_COMMAND = 20.0
-
-    object TurnGains {
-      const val KP = 8.0
-      const val KI = 0.0
-      const val KD = 0.8
-
-      val MAX_VELOCITY = 90.0.degrees.perSecond
-      val MAX_ACCEL = 450.0.degrees.perSecond.perSecond
-    }
-  }
-
-  object Climber {
-    val R_ARM_ID = 62
-    val L_ARM_ID = 61
-    val CLIMBER_SENSOR_LINEARMECH_GEARRATIO = 1 / 8.4
-    val CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER = 1.inches // diameter: .0508 meters = 2 in
-    val CLIMBER_SOLENOID_ID = 5
-    val CLIMBER_P = 0.1
-    val CLIMBER_I = 0.1
-    val CLIMBER_D = 0.1
-    val CLIMBER_SPARKMAX_VEL = 0.5.meters.perSecond
-    val CLIMBER_SPARKMAX_ACC = 0.5.meters.perSecond.perSecond
-    val BRAKE_RELEASE_TIMEOUT = 0.3.seconds
-
-    val BOTTOM_SAFETY_THRESHOLD = 0.inches
-    val TOP_SAFETY_THRESHOLD = 34.inches
-
-    const val POSITION_P = 1.0 / 50.0
-
-    const val TAB = "Climber"
-  }
-
-  enum class ClimberPosition(val length: Length) {
-    LOW(0.meters),
-    HIGH(1.0414.meters) // Climber fulled extended: 1.0414 meters = 41 in
-  }
-
-  object RobotPositions {
-    val START_X = 108.9135.inches // TODO: Determine if accurate
-    val START_Y = 94.655.inches // TODO: Determine if this is accurate and will make bot aligned
-    val START_ANGLE = 180.degrees
-
-    val CROSS_BAR_X = 5.0.meters
-    val CROSS_BAR_Y = 4.7.meters
-    val CROSS_BAR_ANGLE = 135.degrees // TODO: Find ending angle
-
-    val AVOID_BAR_X = 6.7.meters
-    val AVOID_BAR_Y = 2.0.meters
-    val AVOID_BAR_ANGLE = 180.degrees // TODO: Find ending angle
-  }
-
-  object BallVision {
-    val CENTER_YAW_THRESHOLD = 6.degrees
-    val PATH_A_AREA_THRESHOLD = 0.1
-  }
-}
+package com.team4099.robot2021.config
+
+import com.team4099.lib.units.base.Length
+import com.team4099.lib.units.base.feet
+import com.team4099.lib.units.base.inches
+import com.team4099.lib.units.base.meters
+import com.team4099.lib.units.base.seconds
+import com.team4099.lib.units.derived.degrees
+import com.team4099.lib.units.derived.div
+import com.team4099.lib.units.derived.rotations
+import com.team4099.lib.units.derived.volts
+import com.team4099.lib.units.perMinute
+import com.team4099.lib.units.perSecond
+import edu.wpi.first.wpilibj.DoubleSolenoid
+
+object Constants {
+  object Universal {
+    const val CTRE_CONFIG_TIMEOUT = 0
+    const val EPSILON = 1E-9
+  }
+
+  object Tuning {
+    const val TUNING_TOGGLE_PIN = 0
+    val ROBOT_ID_PINS = 1..2
+
+    enum class RobotName {
+      COMPETITION,
+      PRACTICE,
+      MULE
+    }
+
+    val ROBOT_ID_MAP =
+        mapOf<Int, RobotName>(
+            0 to RobotName.COMPETITION, 1 to RobotName.PRACTICE, 2 to RobotName.MULE)
+  }
+
+  object Joysticks {
+    const val DRIVER_PORT = 0
+    const val SHOTGUN_PORT = 1
+    const val TECHNICIAN_PORT = 2
+
+    const val THROTTLE_DEADBAND = 0.05
+    const val TURN_DEADBAND = 0.05
+  }
+
+  object Drivetrain {
+    const val TICKS = 4096
+
+    const val FRONT_LEFT_SPEED_ID = 11
+    const val FRONT_LEFT_DIRECTION_ID = 21
+    const val FRONT_LEFT_CANCODER_ID = 2
+
+    const val FRONT_RIGHT_SPEED_ID = 12
+    const val FRONT_RIGHT_DIRECTION_ID = 22
+    const val FRONT_RIGHT_CANCODER_ID = 1
+
+    const val BACK_RIGHT_SPEED_ID = 13
+    const val BACK_RIGHT_DIRECTION_ID = 23
+    const val BACK_RIGHT_CANCODER_ID = 4
+
+    const val BACK_LEFT_SPEED_ID = 14
+    const val BACK_LEFT_DIRECTION_ID = 24
+    const val BACK_LEFT_CANCODER_ID = 3
+
+    const val WHEEL_COUNT = 4
+    val DRIVETRAIN_LENGTH = 22.173.inches
+    val DRIVETRAIN_WIDTH = 22.173.inches
+
+    val DRIVE_SETPOINT_MAX = 15.feet.perSecond
+    val TURN_SETPOINT_MAX = 360.degrees.perSecond // TODO: Make sure this value is something good
+
+    val DIRECTION_VEL_MAX = 900.degrees.perSecond
+    val DIRECTION_ACCEL_MAX = 4500.degrees.perSecond.perSecond
+
+    const val GYRO_RATE_COEFFICIENT = 0.0 // TODO: Change this value
+
+    val MAX_AUTO_VEL = 3.meters.perSecond
+    val SLOW_AUTO_VEL = 0.66.meters.perSecond
+    val MAX_AUTO_ACCEL = 2.0.meters.perSecond.perSecond
+
+    val MAX_AUTO_ANGULAR_VEL = 90.0.degrees.perSecond
+    val MAX_AUTO_ANGULAR_ACCEL = 90.0.degrees.perSecond.perSecond
+
+    const val ABSOLUTE_GEAR_RATIO = 1.0
+    const val DRIVE_SENSOR_GEAR_RATIO = (12.0 / 21.0) * (15.0 / 45.0)
+    const val DIRECTION_SENSOR_GEAR_RATIO = (12.0 / 64.0) * (1.0 / 10.0)
+
+    val ALLOWED_ANGLE_ERROR = 1.degrees
+    const val DIRECTION_SMART_CURRENT_LIMIT = 20
+    const val DRIVE_SMART_CURRENT_LIMIT = 80
+
+    object Gains {
+      const val RAMSETE_B = 2.0
+      const val RAMSETE_ZETA = 0.7
+    }
+
+    object PID {
+      const val DIRECTION_KP = 0.00001
+      const val DIRECTION_KI = 0.0
+      const val DIRECTION_KD = 12.0
+      const val DIRECTION_KFF = 0.000078
+
+      const val DRIVE_KP = 0.000129
+      const val DRIVE_KI = 0.0
+      const val DRIVE_KD = 0.0
+      const val DRIVE_KFF = 0.0
+
+      const val AUTO_POS_KP = 0.581 / 6
+      const val AUTO_POS_KI = 0.0
+      const val AUTO_POS_KD = 0.0 // 263.0 / 6
+
+      const val DRIVE_THETA_PID_KP = 1.0
+      const val DRIVE_THETA_PID_KI = 0.0
+      const val DRIVE_THETA_PID_KD = 0.0
+      val DRIVE_THETA_PID_MAX_VEL = 0.0.meters.perSecond
+      val DRIVE_THETA_PID_MAX_ACCEL = 0.0.meters.perSecond.perSecond
+
+      val DRIVE_KS = 0.339.volts
+      val DRIVE_KV = 2.78.volts / 1.0.meters.perSecond
+      val DRIVE_KA = 0.421.volts / 1.0.meters.perSecond.perSecond
+    }
+  }
+
+  object Feeder {
+    const val FLOOR_ID = 41
+    const val VERTICAL_ID = 42
+    const val FEEDER_POWER = 1.0 // was 0.45
+    const val FAST_FEEDER_POWER = 0.75 // was 1.0
+    // add one for close and far shot
+    const val FLOOR_CURRENT_LIMIT = 15
+    const val VERTICAL_CURRENT_LIMIT = 40
+
+    const val TOP_DIO_PIN = 9
+    const val BOTTOM_DIO_PIN = 8
+    const val BEAM_BREAK_BROKEN_TIME = 0.05
+    const val BEAM_BREAK_BACKWARDS_TIME = 0.05
+    const val TAB = "Feeder"
+  }
+
+  object Intake {
+    const val INTAKE_MOTOR = 31
+    const val ARM_SOLENOID_FORWARD = 7
+    const val ARM_SOLENOID_REVERSE = 0
+    const val TAB = "Intake"
+
+    val RAMP_TIME = 1.0
+
+    enum class IntakeState(val speed: Double) {
+      IDLE(0.0),
+      IN(1.0),
+      OUT(-1.0)
+    }
+
+    enum class ArmPosition(val position: DoubleSolenoid.Value?) {
+      OUT(DoubleSolenoid.Value.kReverse),
+      IN(DoubleSolenoid.Value.kForward)
+    }
+  }
+
+  object Shooter {
+    const val SHOOTER_MOTOR_ID = 51
+    const val SHOOTER_FOLLOWER_ID = 52
+
+    const val SOLENOID_FORWARD_CHANNEL = 1
+    const val SOLENOID_REVERSE_CHANNEL = 6
+
+    val VELOCITY_TOLERANCE = 100.rotations.perMinute
+
+    const val SHOOTER_KS = 0.939 // * 2
+    const val SHOOTER_KV = 0.1 // 0.114 // * 2
+
+    const val SHOOTER_KP = 0.5 // 0.65
+    const val SHOOTER_KI = 0.0
+    const val SHOOTER_KD = 0.0
+
+    val NEAR_VELOCITY = 1700.0.rotations.perMinute
+    val LINE_VELOCITY = 2750.0.rotations.perMinute
+    val MID_VELOCITY = 2800.0.rotations.perMinute
+    val FAR_VELOCITY = 4000.0.rotations.perMinute
+    // TODO: Determine velocity needed to shoot from front of trench
+
+    val LINE_DISTANCE = 100.0.inches
+    val NEAR_DISTANCE = 130.0.inches
+    val MID_DISTANCE = 249.0.inches
+
+    val POWER_CELL_CHALLENGE_RPM = 2950.rotations.perMinute
+
+    val UNJAM_RPM = 200.rotations.perMinute
+
+    // val HOOD_THRESHOLD = 0.0.inches
+
+    const val TAB = "Shooter"
+  }
+
+  object Vision {
+    const val DRIVER_PIPELINE_ID = 1
+    const val TARGETING_PIPELINE_ID = 0
+    val TARGET_HEIGHT = 98.25.inches
+
+    val CLOSE_CAMERA_HEIGHT = 0.0.inches
+    val CLOSE_CAMERA_ANGLE = 0.0.degrees
+    val FAR_CAMERA_HEIGHT = 35.0.inches
+    val FAR_CAMERA_ANGLE = 24.0.degrees
+
+    val CAMERA_DIST_THRESHOLD = 55.0.inches
+
+    val MAX_DIST_ERROR = 0.1.inches
+    val MAX_ANGLE_ERROR = 1.0.degrees
+
+    const val MIN_TURN_COMMAND = 20.0
+
+    object TurnGains {
+      const val KP = 8.0
+      const val KI = 0.0
+      const val KD = 0.8
+
+      val MAX_VELOCITY = 90.0.degrees.perSecond
+      val MAX_ACCEL = 450.0.degrees.perSecond.perSecond
+    }
+  }
+
+  object Climber {
+    val R_ARM_ID = 62
+    val L_ARM_ID = 61
+    val CLIMBER_SENSOR_LINEARMECH_GEARRATIO = 1 / 8.4
+    val CLIMBER_SENSOR_LINEARMECH_PULLEYDIAMETER = 1.inches // diameter: .0508 meters = 2 in
+    val CLIMBER_SOLENOID_ID = 5
+    val CLIMBER_P = 0.1
+    val CLIMBER_I = 0.1
+    val CLIMBER_D = 0.1
+    val CLIMBER_SPARKMAX_VEL = 0.5.meters.perSecond
+    val CLIMBER_SPARKMAX_ACC = 0.5.meters.perSecond.perSecond
+    val BRAKE_RELEASE_TIMEOUT = 0.3.seconds
+
+    val BOTTOM_SAFETY_THRESHOLD = 0.inches
+    val TOP_SAFETY_THRESHOLD = 34.inches
+
+    const val POSITION_P = 1.0 / 50.0
+
+    const val TAB = "Climber"
+  }
+
+  enum class ClimberPosition(val length: Length) {
+    LOW(0.meters),
+    HIGH(1.0414.meters) // Climber fulled extended: 1.0414 meters = 41 in
+  }
+
+  object RobotPositions {
+    val START_X = 108.9135.inches // TODO: Determine if accurate
+    val START_Y = 94.655.inches // TODO: Determine if this is accurate and will make bot aligned
+    val START_ANGLE = 180.degrees
+
+    val CROSS_BAR_X = 5.0.meters
+    val CROSS_BAR_Y = 4.7.meters
+    val CROSS_BAR_ANGLE = 135.degrees // TODO: Find ending angle
+
+    val AVOID_BAR_X = 6.7.meters
+    val AVOID_BAR_Y = 2.0.meters
+    val AVOID_BAR_ANGLE = 180.degrees // TODO: Find ending angle
+  }
+
+  object BallVision {
+    val CENTER_YAW_THRESHOLD = 6.degrees
+    val PATH_A_AREA_THRESHOLD = 0.1
+  }
+
+  object LED {
+    const val PORT = 0 // PWM Port (could be Arduino)
+    const val LED_COUNT = 50 // TODO: Determine total amount of LED
+    const val STATUS_LENGTH = 0 // TODO: Determine length for health
+
+    enum class Status(var h: Int, var s: Int, var v: Int) {
+      // Green
+      VISION_LOCK(52, 255, 255),
+
+      // Blue
+      SHOOTER_SPEED(120, 255, 255),
+
+      // White
+      INTAKE_EMPTY(0, 0, 255),
+
+      // Magenta
+      ONE_TWO_BALL(154, 255, 255),
+
+      // Sky blue
+      THREE_FOUR_BALL(90, 255, 255),
+
+      // Weird yellow green
+      DEFAULT(36, 255, 255)
+    }
+
+    enum class Health(var h: Int, var s: Int, var v: Int) {
+      VISION(0, 255, 255),
+      BEAM_BREAK(60, 255, 255),
+      OTHER(17, 255, 255),
+      DEFAULT(36, 255, 255)
+    }
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt
index 9a071442..4e0c4b3d 100644
--- a/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt
+++ b/src/main/kotlin/com/team4099/robot2021/subsystems/Feeder.kt
@@ -3,6 +3,7 @@ package com.team4099.robot2021.subsystems
 import com.revrobotics.CANSparkMax
 import com.revrobotics.CANSparkMaxLowLevel
 import com.team4099.lib.logging.Logger
+import com.team4099.robot2021.commands.led.LEDCommand
 import com.team4099.robot2021.config.Constants
 import com.team4099.robot2021.config.Constants.Feeder.FLOOR_CURRENT_LIMIT
 import com.team4099.robot2021.config.Constants.Feeder.VERTICAL_CURRENT_LIMIT
@@ -70,6 +71,17 @@ object Feeder : SubsystemBase() {
     if (topLastStage != topBeamBroken && !topBeamBroken && verticalMotor.appliedOutput > 0) {
       ballCount--
     }
+    when {
+      ballCount <= 0 -> {
+        LEDCommand(Constants.LED.Status.INTAKE_EMPTY)
+      }
+      ballCount in 1..2 -> {
+        LEDCommand(Constants.LED.Status.ONE_TWO_BALL)
+      }
+      else -> {
+        LEDCommand(Constants.LED.Status.THREE_FOUR_BALL)
+      }
+    }
     bottomLastStage = bottomBeamBroken
     topLastStage = topBeamBroken
   }
diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/LED.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/LED.kt
new file mode 100644
index 00000000..f0d7f292
--- /dev/null
+++ b/src/main/kotlin/com/team4099/robot2021/subsystems/LED.kt
@@ -0,0 +1,68 @@
+package com.team4099.robot2021.subsystems
+
+import com.team4099.lib.logging.Logger
+import com.team4099.robot2021.config.Constants
+import edu.wpi.first.wpilibj.AddressableLED
+import edu.wpi.first.wpilibj.AddressableLEDBuffer
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+
+object LED : SubsystemBase() {
+  // using https://www.amazon.com/ALITOVE-Individually-Addressable-Flexible-Waterproof/dp/B018X04ES2
+
+  private val led = AddressableLED(Constants.LED.PORT)
+  private val ledBuffer = AddressableLEDBuffer(Constants.LED.LED_COUNT)
+  private var checkBall = true
+  private var rainbowFirstPixelHue = 0
+
+  var statusState = Constants.LED.Status.DEFAULT
+    set(value) {
+      if (value == Constants.LED.Status.ONE_TWO_BALL ||
+          value == Constants.LED.Status.THREE_FOUR_BALL) {
+        if (checkBall) {
+          for (i in 0 until Constants.LED.STATUS_LENGTH) {
+            ledBuffer.setHSV(i, value.h, value.s, value.v)
+          }
+          field = value
+        }
+      } else {
+        checkBall = false
+        if (value == Constants.LED.Status.INTAKE_EMPTY) {
+          checkBall = true
+        }
+        for (i in 0 until Constants.LED.STATUS_LENGTH) {
+          ledBuffer.setHSV(i, value.h, value.s, value.v)
+        }
+        field = value
+      }
+    }
+
+  var healthState = Constants.LED.Health.DEFAULT
+    set(value) {
+      for (i in Constants.LED.STATUS_LENGTH + 1 until ledBuffer.length) {
+        ledBuffer.setHSV(i, value.h, value.s, value.v)
+      }
+      field = value
+    }
+
+  init {
+    led.setLength(ledBuffer.length)
+    led.setData(ledBuffer)
+    led.start()
+
+    Logger.addSource("LED", "LED Status") { statusState }
+  }
+  fun rainbow() {
+    // For every pixel
+    for (i in 0 until Constants.LED.STATUS_LENGTH) {
+      // Calculate the hue - hue is easier for rainbows because the color
+      // shape is a circle so only one value needs to precess
+      val hue = (rainbowFirstPixelHue + i * 180 / ledBuffer.length) % 180
+      // Set the value
+      ledBuffer.setHSV(i, hue, 255, 128)
+    }
+    // Increase by to make the rainbow "move"
+    rainbowFirstPixelHue += 3
+    // Check bounds
+    rainbowFirstPixelHue %= 180
+  }
+}
diff --git a/src/main/kotlin/com/team4099/robot2021/subsystems/Shooter.kt b/src/main/kotlin/com/team4099/robot2021/subsystems/Shooter.kt
index 7b418f7f..994f3814 100644
--- a/src/main/kotlin/com/team4099/robot2021/subsystems/Shooter.kt
+++ b/src/main/kotlin/com/team4099/robot2021/subsystems/Shooter.kt
@@ -104,7 +104,7 @@ object Shooter : SubsystemBase() {
   // velocity setpoint
   private var _targetVelocity = 0.rotations.perMinute
   var targetVelocity
-    set(velocity) {
+    set(velocity) { // TODO: Set LED when reaches target velocity
       _targetVelocity = velocity
       shooterMotor.set(
           ControlMode.Velocity,