From c1ecbcf7516d54d009cbeac304fc9d1a52185aae Mon Sep 17 00:00:00 2001
From: Shom770 <shayaanwadkar@gmail.com>
Date: Mon, 1 Apr 2024 01:34:39 -0400
Subject: [PATCH 1/7] working battery voltage status bar

---
 .../com/team4099/robot2023/RobotContainer.kt  |  6 +-
 .../config/constants/IntakeConstants.kt       |  7 +-
 .../config/constants/LEDConstants.kt          | 13 ++-
 .../config/constants/WristConstants.kt        |  2 +-
 .../subsystems/elevator/ElevatorIO.kt         |  2 +-
 .../subsystems/intake/IntakeIOFalconNEO.kt    | 17 ++--
 .../robot2023/subsystems/led/LedIO.kt         |  3 +
 .../robot2023/subsystems/led/LedIOCandle.kt   | 96 ++++++++++++++++++-
 .../team4099/robot2023/subsystems/led/Leds.kt | 25 ++---
 .../superstructure/Superstructure.kt          |  2 -
 .../robot2023/subsystems/wrist/WristIO.kt     |  2 +-
 11 files changed, 142 insertions(+), 33 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 9c96230a..eabd800f 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -15,6 +15,7 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
+import com.team4099.robot2023.subsystems.elevator.ElevatorIO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
@@ -34,6 +35,7 @@ import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIO
 import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.subsystems.wrist.Wrist
+import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
 import com.team4099.robot2023.subsystems.wrist.WristIOTalon
 import com.team4099.robot2023.util.driver.Ryan
@@ -80,9 +82,9 @@ object RobotContainer {
       limelight = LimelightVision(object : LimelightVisionIO {})
       intake = Intake(IntakeIOFalconNEO)
       feeder = Feeder(FeederIONeo)
-      elevator = Elevator(ElevatorIONEO)
+      elevator = Elevator(object : ElevatorIO {})
       flywheel = Flywheel(FlywheelIOTalon)
-      wrist = Wrist(WristIOTalon)
+      wrist = Wrist(object : WristIO {})
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
index 0682b554..47b13607 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt
@@ -4,6 +4,7 @@ import org.team4099.lib.geometry.Transform2d
 import org.team4099.lib.geometry.Translation2d
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.seconds
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.volts
 
@@ -17,7 +18,11 @@ object IntakeConstants {
   val INTAKE_TRANSFORM = Transform2d(Translation2d(-18.0.inches, 0.0.inches), 0.0.degrees)
 
   // TODO: Change gear ratio according to robot
-  val ROLLER_CURRENT_LIMIT = 80.0.amps
+  val ROLLER_CURRENT_LIMIT = 80.amps
+  val ROLLER_SUPPLY_CURRENT_LIMIT = 120.0.amps
+  val ROLLER_STATOR_CURRENT_LIMIT = 200.0.amps
+  val ROLLER_CURRENT_TIME_THRESHOLD = 1.5.seconds
+  val ROLLER_SUPPLY_TRIGGER_THRESHOLD = 65.amps
   const val ROLLER_MOTOR_INVERTED = true
   const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated
   const val CENTER_WHEEL_GEAR_RATIO = 34.0 / 14.0
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
index 4d135228..45eafef3 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
@@ -7,23 +7,26 @@ import org.team4099.lib.units.derived.volts
 object LEDConstants {
   val INTAKE_CURRENT_THRESHOLD = 15.amps
   val OUTAKE_CURRENT_THRESHOLD = 20.amps
+  val BATTERY_FULL_THRESHOLD = 12.5.volts
   val LED_COUNT = 50
 
-  val BATTERY_WARNING_THRESHOLD = 12.3.volts
-
-  enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int) {
+  enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int, var attachedValue: Any? = null) {
     // Gold
     NO_NOTE(null, 0, 0, 0),
     NOTHING(null, 0, 0, 0),
     RED(null, 255, 0, 0),
     BLUE(null, 0, 0, 255),
+    GREEN(null, 0, 255, 0),
+    PURPLE(null, 160, 32, 240),
+    MAGENTA(null, 255, 0, 255),
 
     // Blue
 
     HAS_NOTE(null, 0, 0, 255),
 
-    // Red
-    LOW_BATTERY(null, 255, 105, 0),
+    // Yellow
+    BATTERY_DISPLAY(null, 255, 105, 0),
+    WHITE(null, 255, 255, 255),
 
     // Green
 
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
index 8a193c77..37e35967 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/WristConstants.kt
@@ -31,7 +31,7 @@ object WristConstants {
 
   val ABSOLUTE_ENCODER_TO_MECHANISM_GEAR_RATIO = 1.06488 / 1.0
   val MOTOR_TO_ABSOLUTE_ENCODER_GEAR_RATIO =
-    5.0 / 1.0 * 4.0 / 1.0 * 3.0 / 1.0 * 46.0 / 42.0 * 90.0 / 33.0 * 1.0 / 1.06488
+    5.0 / 1.0 * 4.0 / 1.0 * 54.0 / 34.0 * 90.0 / 33.0 * 1.0 / 1.06488
 
   val VOLTAGE_COMPENSATION = 12.0.volts
   val ABSOLUTE_ENCODER_OFFSET =
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
index a5e20665..28013a0a 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt
@@ -37,7 +37,7 @@ interface ElevatorIO {
     var leaderTempCelcius = 0.0.celsius
     var followerTempCelcius = 0.0.celsius
 
-    var isSimulating = false
+    var isSimulating = true
 
     override fun toLog(table: LogTable) {
       table?.put("elevatorPositionInches", elevatorPosition.inInches)
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
index 50c171c3..acc1e27c 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
@@ -20,6 +20,7 @@ import com.team4099.robot2023.subsystems.falconspin.Neo
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inAmperes
+import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.ctreAngularMechanismSensor
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.inVolts
@@ -56,11 +57,13 @@ object IntakeIOFalconNEO : IntakeIO {
     var rollerFalconConfiguration = TalonFXConfiguration()
 
     rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit =
-      IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes
-    rollerFalconConfiguration.CurrentLimits.StatorCurrentLimit =
-      IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes
+      IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes
+    rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit =
+      IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes
+//    rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds
+//    rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes
     rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true
-    rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = false
+    rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
     rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
     rollerFalconConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Coast
 
@@ -76,7 +79,7 @@ object IntakeIOFalconNEO : IntakeIO {
       "Roller",
       MotorCollection(
         mutableListOf(Falcon500(rollerFalcon, "Roller Motor")),
-        IntakeConstants.ROLLER_CURRENT_LIMIT,
+        IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT,
         60.celsius,
         50.amps,
         120.celsius
@@ -87,7 +90,7 @@ object IntakeIOFalconNEO : IntakeIO {
     centerWheelSparkMax.clearFaults()
 
     centerWheelSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts)
-    centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt())
+    // centerWheelSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt())
     centerWheelSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED
 
     centerWheelSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
@@ -99,7 +102,7 @@ object IntakeIOFalconNEO : IntakeIO {
       "Center Wheel",
       MotorCollection(
         mutableListOf(Neo(centerWheelSparkMax, "Center Wheel Motor")),
-        IntakeConstants.ROLLER_CURRENT_LIMIT,
+        IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT,
         60.celsius,
         50.amps,
         120.celsius
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt
index 26ac3c09..0ede25f5 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIO.kt
@@ -3,8 +3,11 @@ package com.team4099.robot2023.subsystems.led
 import com.team4099.robot2023.config.constants.LEDConstants
 import org.littletonrobotics.junction.LogTable
 import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.units.derived.ElectricalPotential
 
 interface LedIO {
+  var batteryVoltage: ElectricalPotential
+
   class LedIOInputs : LoggableInputs {
     var ledState = LEDConstants.CandleState.NO_NOTE.toString()
 
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
index e14e188b..21dbc4a5 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
@@ -4,11 +4,17 @@ import com.ctre.phoenix.led.CANdle
 import com.team4099.robot2023.config.constants.Constants
 import com.team4099.robot2023.config.constants.LEDConstants
 import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
 
 object LedIOCandle : LedIO {
 
   private val ledController = CANdle(Constants.LED.LED_CANDLE_ID)
   private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE
+  private var runs = 0
+  private var reverseLEDS = false
+  override var batteryVoltage: ElectricalPotential = 12.0.volts
 
   override fun updateInputs(inputs: LedIO.LedIOInputs) {
     inputs.ledState = lastState.name
@@ -20,8 +26,96 @@ object LedIOCandle : LedIO {
     setCANdleState(newState)
   }
 
+  private fun wave(
+    state: LEDConstants.CandleState,
+    defaultState: LEDConstants.CandleState,
+    lengthOfWave: Int = 10,
+    lengthOfGradient: Double = 5.0,
+  ) {
+    ledController.setLEDs(state.r, state.g, state.b, 0, runs, lengthOfWave)
+
+    // Set outer colors
+    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, runs - lengthOfGradient.toInt())
+    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, runs + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - runs - lengthOfWave)
+
+    // Set gradient colors
+    if (lengthOfGradient > 0.0) {
+      for ((step, ledIdx) in ((runs - lengthOfGradient.toInt())..runs).withIndex()) {
+        ledController.setLEDs(
+          (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(),
+          (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(),
+          (defaultState.b + (state.b - defaultState.b) * ((step + 1) / lengthOfGradient)).toInt(),
+          0,
+          ledIdx,
+          1
+        )
+      }
+
+      for ((step, ledIdx) in (runs..(runs + lengthOfGradient).toInt()).withIndex()) {
+        ledController.setLEDs(
+          (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
+          (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
+          (defaultState.b + (state.b - defaultState.b) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
+          0,
+          ledIdx,
+          1
+        )
+      }
+    }
+
+    if (runs >= LEDConstants.LED_COUNT - lengthOfWave) {
+      reverseLEDS = true
+    }
+    else if (runs < lengthOfWave){
+      reverseLEDS = false
+    }
+
+    runs += if (!reverseLEDS) 1 else -1
+  }
+
+  private fun gradient(
+    state: LEDConstants.CandleState,
+    otherState: LEDConstants.CandleState,
+    lengthOfSolidEnds: Int = 10
+  ) {
+    ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds)
+    ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds)
+
+    for ((step, ledIdx) in (lengthOfSolidEnds..(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) {
+      ledController.setLEDs(
+        (state.r + (otherState.r - state.r) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (state.g + (otherState.g - state.g) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (state.b + (otherState.b - state.b) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        0,
+        ledIdx,
+        1
+      )
+    }
+  }
+
+  private fun progressBar(
+    state: LEDConstants.CandleState,
+    defaultState: LEDConstants.CandleState,
+    percent: Double
+  ) {
+    ledController.setLEDs(state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt())
+    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT)
+  }
+
   private fun setCANdleState(state: LEDConstants.CandleState) {
-    if (state.animation == null) {
+    if (state == LEDConstants.CandleState.BATTERY_DISPLAY) {
+      progressBar(
+        LEDConstants.CandleState.BATTERY_DISPLAY,
+        LEDConstants.CandleState.NOTHING,
+        (batteryVoltage.inVolts - 11.5) / (LEDConstants.BATTERY_FULL_THRESHOLD.inVolts - 11.5)
+      )
+    }
+    else if (state == LEDConstants.CandleState.BLUE) {
+      ledController.clearAnimation(0)
+      val otherState = LEDConstants.CandleState.MAGENTA
+      gradient(state, otherState)
+    }
+    else if (state.animation == null) {
       ledController.clearAnimation(0)
       ledController.setLEDs(state.r, state.g, state.b)
     } else {
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
index 7dd0f7b9..ea14466b 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
@@ -1,9 +1,13 @@
 package com.team4099.robot2023.subsystems.led
 
+import com.team4099.robot2023.RobotContainer
 import com.team4099.robot2023.config.constants.LEDConstants
 import com.team4099.robot2023.util.FMSData
 import edu.wpi.first.wpilibj.DriverStation
+import edu.wpi.first.wpilibj.RobotController
 import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.derived.volts
+import java.sql.Driver
 
 class Leds(val io: LedIO) {
   var inputs = LedIO.LedIOInputs()
@@ -11,7 +15,6 @@ class Leds(val io: LedIO) {
   var hasNote = false
   var subsystemsAtPosition = false
   var isIdle = true
-  var batteryIsLow = false
 
   var state = LEDConstants.CandleState.NO_NOTE
     set(value) {
@@ -21,17 +24,15 @@ class Leds(val io: LedIO) {
 
   fun periodic() {
     io.updateInputs(inputs)
-    if (batteryIsLow && DriverStation.isDisabled()) {
-      state = LEDConstants.CandleState.LOW_BATTERY
-    } else if (DriverStation.isDisabled()) {
-      if (DriverStation.getAlliance().isPresent) {
-        if (FMSData.isBlue) {
-          state = LEDConstants.CandleState.BLUE
-        } else {
-          state = LEDConstants.CandleState.RED
-        }
-      } else {
-        state = LEDConstants.CandleState.NOTHING
+    if (DriverStation.getAlliance().isEmpty) {
+      io.batteryVoltage = RobotController.getBatteryVoltage().volts
+      state = LEDConstants.CandleState.BATTERY_DISPLAY
+    }
+    else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) {
+      if (FMSData.isBlue) {
+        state = LEDConstants.CandleState.BLUE
+      } else  {
+        state = LEDConstants.CandleState.RED
       }
     } else if (hasNote) {
       if (subsystemsAtPosition && !isIdle) {
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index 0b60eca0..e7635db8 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -143,8 +143,6 @@ class Superstructure(
     leds.isIdle = currentState == SuperstructureStates.IDLE
     leds.subsystemsAtPosition =
       wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition
-    leds.batteryIsLow =
-      RobotController.getBatteryVoltage() < LEDConstants.BATTERY_WARNING_THRESHOLD.inVolts
 
     val ledLoopStartTime = Clock.realTimestamp
     leds.periodic()
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt
index d8f031a0..4607448c 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt
@@ -37,7 +37,7 @@ interface WristIO {
 
     var wristAcceleration = 0.0.degrees.perSecond.perSecond
 
-    var isSimulated = false
+    var isSimulated = true
 
     override fun toLog(table: LogTable) {
       table.put("wristPosition", wristPosition.inDegrees)

From 2047e846c07324726fae9c98fd960059e80dade3 Mon Sep 17 00:00:00 2001
From: Shom770 <shayaanwadkar@gmail.com>
Date: Mon, 1 Apr 2024 02:09:35 -0400
Subject: [PATCH 2/7] more led stuff

---
 .../com/team4099/utils/LimelightHelpers.java  | 228 +++++++++++++++++-
 .../com/team4099/robot2023/RobotContainer.kt  |   3 +-
 .../robot2023/config/constants/Constants.kt   |   4 +-
 .../config/constants/VisionConstants.kt       |   2 +-
 .../robot2023/subsystems/led/LedIOCandle.kt   |  50 ++--
 .../subsystems/limelight/LimelightVision.kt   |   1 +
 6 files changed, 262 insertions(+), 26 deletions(-)

diff --git a/src/main/java/com/team4099/utils/LimelightHelpers.java b/src/main/java/com/team4099/utils/LimelightHelpers.java
index 0cb199a3..9ef9ec52 100644
--- a/src/main/java/com/team4099/utils/LimelightHelpers.java
+++ b/src/main/java/com/team4099/utils/LimelightHelpers.java
@@ -1,4 +1,5 @@
-package com.team4099.utils;//com.team4099.utils.LimelightHelpers v1.2.1 (March 1, 2023)
+package com.team4099.utils;//com.team4099.utils.LimelightHelpers
+//LimelightHelpers v1.5.0 (March 27, 2024)
 
 import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
@@ -301,6 +302,18 @@ public static class Results {
         @JsonProperty("botpose_wpiblue")
         public double[] botpose_wpiblue;
 
+        @JsonProperty("botpose_tagcount")
+        public double botpose_tagcount;
+
+        @JsonProperty("botpose_span")
+        public double botpose_span;
+
+        @JsonProperty("botpose_avgdist")
+        public double botpose_avgdist;
+
+        @JsonProperty("botpose_avgarea")
+        public double botpose_avgarea;
+
         @JsonProperty("t6c_rs")
         public double[] camerapose_robotspace;
 
@@ -361,8 +374,59 @@ public static class LimelightResults {
         @JsonProperty("Results")
         public Results targetingResults;
 
+        public String error;
+
         public LimelightResults() {
             targetingResults = new Results();
+            error = "";
+        }
+
+
+    }
+
+    public static class RawFiducial {
+        public int id;
+        public double txnc;
+        public double tync;
+        public double ta;
+        public double distToCamera;
+        public double distToRobot;
+        public double ambiguity;
+
+
+        public RawFiducial(int id, double txnc, double tync, double ta, double distToCamera, double distToRobot, double ambiguity) {
+            this.id = id;
+            this.txnc = txnc;
+            this.tync = tync;
+            this.ta = ta;
+            this.distToCamera = distToCamera;
+            this.distToRobot = distToRobot;
+            this.ambiguity = ambiguity;
+        }
+    }
+
+    public static class PoseEstimate {
+        public Pose2d pose;
+        public double timestampSeconds;
+        public double latency;
+        public int tagCount;
+        public double tagSpan;
+        public double avgTagDist;
+        public double avgTagArea;
+        public RawFiducial[] rawFiducials;
+
+        public PoseEstimate(Pose2d pose, double timestampSeconds, double latency,
+                            int tagCount, double tagSpan, double avgTagDist,
+                            double avgTagArea, RawFiducial[] rawFiducials) {
+
+            this.pose = pose;
+            this.timestampSeconds = timestampSeconds;
+            this.latency = latency;
+            this.tagCount = tagCount;
+            this.tagSpan = tagSpan;
+            this.avgTagDist = avgTagDist;
+            this.avgTagArea = avgTagArea;
+            this.rawFiducials = rawFiducials;
         }
     }
 
@@ -383,7 +447,7 @@ static final String sanitizeName(String name) {
     private static Pose3d toPose3D(double[] inData){
         if(inData.length < 6)
         {
-            System.err.println("Bad LL 3D Pose Data!");
+            //System.err.println("Bad LL 3D Pose Data!");
             return new Pose3d();
         }
         return new Pose3d(
@@ -395,7 +459,7 @@ private static Pose3d toPose3D(double[] inData){
     private static Pose2d toPose2D(double[] inData){
         if(inData.length < 6)
         {
-            System.err.println("Bad LL 2D Pose Data!");
+            //System.err.println("Bad LL 2D Pose Data!");
             return new Pose2d();
         }
         Translation2d tran2d = new Translation2d(inData[0], inData[1]);
@@ -403,6 +467,86 @@ private static Pose2d toPose2D(double[] inData){
         return new Pose2d(tran2d, r2d);
     }
 
+    private static double extractBotPoseEntry(double[] inData, int position){
+        if(inData.length < position+1)
+        {
+            return 0;
+        }
+        return inData[position];
+    }
+
+    private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) {
+        var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName);
+        var poseArray = poseEntry.getDoubleArray(new double[0]);
+        var pose = toPose2D(poseArray);
+        double latency = extractBotPoseEntry(poseArray,6);
+        int tagCount = (int)extractBotPoseEntry(poseArray,7);
+        double tagSpan = extractBotPoseEntry(poseArray,8);
+        double tagDist = extractBotPoseEntry(poseArray,9);
+        double tagArea = extractBotPoseEntry(poseArray,10);
+        //getlastchange() in microseconds, ll latency in milliseconds
+        var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency/1000.0);
+
+
+        RawFiducial[] rawFiducials = new RawFiducial[tagCount];
+        int valsPerFiducial = 7;
+        int expectedTotalVals = 11 + valsPerFiducial*tagCount;
+
+        if (poseArray.length != expectedTotalVals) {
+            // Don't populate fiducials
+        }
+        else{
+            for(int i = 0; i < tagCount; i++) {
+                int baseIndex = 11 + (i * valsPerFiducial);
+                int id = (int)poseArray[baseIndex];
+                double txnc = poseArray[baseIndex + 1];
+                double tync = poseArray[baseIndex + 2];
+                double ta = poseArray[baseIndex + 3];
+                double distToCamera = poseArray[baseIndex + 4];
+                double distToRobot = poseArray[baseIndex + 5];
+                double ambiguity = poseArray[baseIndex + 6];
+                rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity);
+            }
+        }
+
+        return new PoseEstimate(pose, timestamp,latency,tagCount,tagSpan,tagDist,tagArea,rawFiducials);
+    }
+
+    private static void printPoseEstimate(PoseEstimate pose) {
+        if (pose == null) {
+            System.out.println("No PoseEstimate available.");
+            return;
+        }
+
+        System.out.printf("Pose Estimate Information:%n");
+        System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds);
+        System.out.printf("Latency: %.3f ms%n", pose.latency);
+        System.out.printf("Tag Count: %d%n", pose.tagCount);
+        System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan);
+        System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist);
+        System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea);
+        System.out.println();
+
+        if (pose.rawFiducials == null || pose.rawFiducials.length == 0) {
+            System.out.println("No RawFiducials data available.");
+            return;
+        }
+
+        System.out.println("Raw Fiducials Details:");
+        for (int i = 0; i < pose.rawFiducials.length; i++) {
+            RawFiducial fiducial = pose.rawFiducials[i];
+            System.out.printf(" Fiducial #%d:%n", i + 1);
+            System.out.printf("  ID: %d%n", fiducial.id);
+            System.out.printf("  TXNC: %.2f%n", fiducial.txnc);
+            System.out.printf("  TYNC: %.2f%n", fiducial.tync);
+            System.out.printf("  TA: %.2f%n", fiducial.ta);
+            System.out.printf("  Distance to Camera: %.2f meters%n", fiducial.distToCamera);
+            System.out.printf("  Distance to Robot: %.2f meters%n", fiducial.distToRobot);
+            System.out.printf("  Ambiguity: %.2f%n", fiducial.ambiguity);
+            System.out.println();
+        }
+    }
+
     public static NetworkTable getLimelightNTTable(String tableName) {
         return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName));
     }
@@ -542,8 +686,8 @@ public static double getFiducialID(String limelightName) {
         return getLimelightNTDouble(limelightName, "tid");
     }
 
-    public static double getNeuralClassID(String limelightName) {
-        return getLimelightNTDouble(limelightName, "tclass");
+    public static String getNeuralClassID(String limelightName) {
+        return getLimelightNTString(limelightName, "tclass");
     }
 
     /////
@@ -602,6 +746,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) {
         return toPose2D(result);
     }
 
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     *
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpiblue");
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the BLUE
+     * alliance
+     *
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue");
+    }
+
     /**
      * Gets the Pose2d for easy use with Odometry vision pose estimator
      * (addVisionMeasurement)
@@ -616,6 +782,26 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) {
 
     }
 
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_wpired");
+    }
+
+    /**
+     * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when you are on the RED
+     * alliance
+     * @param limelightName
+     * @return
+     */
+    public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) {
+        return getBotPoseEstimate(limelightName, "botpose_orb_wpired");
+    }
+
     /**
      * Gets the Pose2d for easy use with Odometry vision pose estimator
      * (addVisionMeasurement)
@@ -641,6 +827,11 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) {
         setLimelightNTDouble(limelightName, "pipeline", pipelineIndex);
     }
 
+
+    public static void setPriorityTagID(String limelightName, int ID) {
+        setLimelightNTDouble(limelightName, "priorityid", ID);
+    }
+
     /**
      * The LEDs will be controlled by Limelight pipeline settings, and not by robot
      * code.
@@ -694,6 +885,28 @@ public static void setCropWindow(String limelightName, double cropXMin, double c
         setLimelightNTDoubleArray(limelightName, "crop", entries);
     }
 
+    public static void SetRobotOrientation(String limelightName, double yaw, double yawRate,
+                                           double pitch, double pitchRate,
+                                           double roll, double rollRate) {
+
+        double[] entries = new double[6];
+        entries[0] = yaw;
+        entries[1] = yawRate;
+        entries[2] = pitch;
+        entries[3] = pitchRate;
+        entries[4] = roll;
+        entries[5] = rollRate;
+        setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries);
+    }
+
+    public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) {
+        double[] validIDsDouble = new double[validIDs.length];
+        for (int i = 0; i < validIDs.length; i++) {
+            validIDsDouble[i] = validIDs[i];
+        }
+        setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble);
+    }
+
     public static void setCameraPose_RobotSpace(String limelightName, double forward, double side, double up, double roll, double pitch, double yaw) {
         double[] entries = new double[6];
         entries[0] = forward;
@@ -763,7 +976,7 @@ public static LimelightResults getLatestResults(String limelightName) {
         try {
             results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class);
         } catch (JsonProcessingException e) {
-            System.err.println("lljson error: " + e.getMessage());
+            results.error = "lljson error: " + e.getMessage();
         }
 
         long end = System.nanoTime();
@@ -775,5 +988,4 @@ public static LimelightResults getLatestResults(String limelightName) {
 
         return results;
     }
-}
-
+}
\ No newline at end of file
diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index eabd800f..a2426c5c 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -29,6 +29,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIOFalconNEO
 import com.team4099.robot2023.subsystems.intake.IntakeIOSim
 import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO
+import com.team4099.robot2023.subsystems.limelight.LimelightVisionIOReal
 import com.team4099.robot2023.subsystems.superstructure.Request
 import com.team4099.robot2023.subsystems.superstructure.Superstructure
 import com.team4099.robot2023.subsystems.vision.Vision
@@ -79,7 +80,7 @@ object RobotContainer {
 
       drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal)
       vision = Vision(object : CameraIO {}, CameraIOPhotonvision("parakeet_2"))
-      limelight = LimelightVision(object : LimelightVisionIO {})
+      limelight = LimelightVision(LimelightVisionIOReal)
       intake = Intake(IntakeIOFalconNEO)
       feeder = Feeder(FeederIONeo)
       elevator = Elevator(object : ElevatorIO {})
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
index 41089e90..c547889c 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -54,8 +54,8 @@ object Constants {
 
   object Tuning {
 
-    const val TUNING_MODE = false
-    const val DEBUGING_MODE = false
+    const val TUNING_MODE = true
+    const val DEBUGING_MODE = true
     const val SIMULATE_DRIFT = false
     const val DRIFT_CONSTANT = 0.001
 
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
index 2903f1aa..babc80e8 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
@@ -77,7 +77,7 @@ object VisionConstants {
   }
 
   object Limelight {
-    val LIMELIGHT_NAME = "limelight-zapdos"
+    val LIMELIGHT_NAME = "limelight-owl"
     val HORIZONTAL_FOV = 59.6.degrees
     val VERITCAL_FOV = 45.7.degrees
     val HIGH_TAPE_HEIGHT = 43.875.inches + 1.inches
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
index 21dbc4a5..9dd2aad4 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
@@ -12,8 +12,10 @@ object LedIOCandle : LedIO {
 
   private val ledController = CANdle(Constants.LED.LED_CANDLE_ID)
   private var lastState: LEDConstants.CandleState = LEDConstants.CandleState.NO_NOTE
-  private var runs = 0
+  private var waveRuns = 0
+  private var loopCycles = 0
   private var reverseLEDS = false
+  private var finishedFade = false
   override var batteryVoltage: ElectricalPotential = 12.0.volts
 
   override fun updateInputs(inputs: LedIO.LedIOInputs) {
@@ -32,15 +34,15 @@ object LedIOCandle : LedIO {
     lengthOfWave: Int = 10,
     lengthOfGradient: Double = 5.0,
   ) {
-    ledController.setLEDs(state.r, state.g, state.b, 0, runs, lengthOfWave)
+    ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave)
 
     // Set outer colors
-    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, runs - lengthOfGradient.toInt())
-    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, runs + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - runs - lengthOfWave)
+    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt())
+    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, waveRuns + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - waveRuns - lengthOfWave)
 
     // Set gradient colors
     if (lengthOfGradient > 0.0) {
-      for ((step, ledIdx) in ((runs - lengthOfGradient.toInt())..runs).withIndex()) {
+      for ((step, ledIdx) in ((waveRuns - lengthOfGradient.toInt())..waveRuns).withIndex()) {
         ledController.setLEDs(
           (defaultState.r + (state.r - defaultState.r) * ((step + 1) / lengthOfGradient)).toInt(),
           (defaultState.g + (state.g - defaultState.g) * ((step + 1) / lengthOfGradient)).toInt(),
@@ -51,7 +53,7 @@ object LedIOCandle : LedIO {
         )
       }
 
-      for ((step, ledIdx) in (runs..(runs + lengthOfGradient).toInt()).withIndex()) {
+      for ((step, ledIdx) in (waveRuns..(waveRuns + lengthOfGradient).toInt()).withIndex()) {
         ledController.setLEDs(
           (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
           (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
@@ -63,14 +65,14 @@ object LedIOCandle : LedIO {
       }
     }
 
-    if (runs >= LEDConstants.LED_COUNT - lengthOfWave) {
+    if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) {
       reverseLEDS = true
     }
-    else if (runs < lengthOfWave){
+    else if (waveRuns < lengthOfWave){
       reverseLEDS = false
     }
 
-    runs += if (!reverseLEDS) 1 else -1
+    waveRuns += if (!reverseLEDS) 1 else -1
   }
 
   private fun gradient(
@@ -81,11 +83,11 @@ object LedIOCandle : LedIO {
     ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds)
     ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds)
 
-    for ((step, ledIdx) in (lengthOfSolidEnds..(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) {
+    for ((step, ledIdx) in (lengthOfSolidEnds until(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) {
       ledController.setLEDs(
-        (state.r + (otherState.r - state.r) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
-        (state.g + (otherState.g - state.g) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
-        (state.b + (otherState.b - state.b) * (step + 1 / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (state.r + (otherState.r - state.r) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (state.g + (otherState.g - state.g) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (state.b + (otherState.b - state.b) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
         0,
         ledIdx,
         1
@@ -102,6 +104,26 @@ object LedIOCandle : LedIO {
     ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT)
   }
 
+  private fun fadeBetweenColors(
+    state: LEDConstants.CandleState,
+    otherState: LEDConstants.CandleState,
+    loopCyclesToConverge: Int = 5
+  ) {
+    for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT).withIndex()) {
+      val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
+      val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
+      val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
+      ledController.setLEDs(
+        calculatedR,
+        calculatedB,
+        calculatedG,
+        0,
+        ledIdx,
+        1
+      )
+    }
+  }
+
   private fun setCANdleState(state: LEDConstants.CandleState) {
     if (state == LEDConstants.CandleState.BATTERY_DISPLAY) {
       progressBar(
@@ -113,7 +135,7 @@ object LedIOCandle : LedIO {
     else if (state == LEDConstants.CandleState.BLUE) {
       ledController.clearAnimation(0)
       val otherState = LEDConstants.CandleState.MAGENTA
-      gradient(state, otherState)
+      fadeBetweenColors(state, otherState)
     }
     else if (state.animation == null) {
       ledController.clearAnimation(0)
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
index 00d19361..495d6841 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -63,6 +63,7 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
   init {}
 
   override fun periodic() {
+    io.setPipeline(0)
     val startTime = Clock.realTimestamp
     io.updateInputs(inputs)
     Logger.processInputs("LimelightVision", inputs)

From d9f698dc9959a76b57a9ac35dbe0c5d4b6140030 Mon Sep 17 00:00:00 2001
From: Shom770 <shayaanwadkar@gmail.com>
Date: Mon, 1 Apr 2024 03:12:04 -0400
Subject: [PATCH 3/7] more led stuff

---
 .../com/team4099/robot2023/RobotContainer.kt  |  1 +
 .../config/constants/LEDConstants.kt          |  3 +-
 .../robot2023/subsystems/led/LedIOCandle.kt   | 32 ++++++++++++++++---
 .../subsystems/limelight/LimelightVision.kt   |  4 ++-
 4 files changed, 33 insertions(+), 7 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index a2426c5c..9ccc73d4 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -164,6 +164,7 @@ object RobotContainer {
   }
 
   fun mapTeleopControls() {
+    limelight.limelightState = LimelightVision.LimelightStates.TARGETING_GAME_PIECE
 
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
     ControlBoard.intake.whileTrue(
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
index 45eafef3..1d64cc73 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
@@ -15,9 +15,10 @@ object LEDConstants {
     NO_NOTE(null, 0, 0, 0),
     NOTHING(null, 0, 0, 0),
     RED(null, 255, 0, 0),
+    LIGHT_RED(null, 255, 67, 36),
     BLUE(null, 0, 0, 255),
+    PURPLE(null, 67, 36, 255),
     GREEN(null, 0, 255, 0),
-    PURPLE(null, 160, 32, 240),
     MAGENTA(null, 255, 0, 255),
 
     // Blue
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
index 9dd2aad4..07973082 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
@@ -7,6 +7,7 @@ import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.volts
+import kotlin.math.absoluteValue
 
 object LedIOCandle : LedIO {
 
@@ -107,21 +108,39 @@ object LedIOCandle : LedIO {
   private fun fadeBetweenColors(
     state: LEDConstants.CandleState,
     otherState: LEDConstants.CandleState,
-    loopCyclesToConverge: Int = 5
+    loopCyclesToConverge: Int = 10
   ) {
-    for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT).withIndex()) {
+    val stepUpInLoopCycles = 2
+    var reachedStartColor = true
+    var reachedEndColor = true
+
+    for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles).withIndex()) {
       val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
       val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
       val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
+
+      reachedStartColor = (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b)
+      reachedEndColor = ((calculatedR - otherState.r).absoluteValue < 10 && (calculatedG - otherState.g).absoluteValue < 10 && (calculatedB - otherState.b).absoluteValue < 10)
+
       ledController.setLEDs(
         calculatedR,
-        calculatedB,
         calculatedG,
+        calculatedB,
         0,
         ledIdx,
         1
       )
     }
+
+    loopCycles += if (finishedFade) -1 else 1
+
+    if (reachedStartColor) {
+      finishedFade = false
+    }
+
+    if (reachedEndColor) { // All LEDs are at the end color
+      finishedFade = true
+    }
   }
 
   private fun setCANdleState(state: LEDConstants.CandleState) {
@@ -134,8 +153,11 @@ object LedIOCandle : LedIO {
     }
     else if (state == LEDConstants.CandleState.BLUE) {
       ledController.clearAnimation(0)
-      val otherState = LEDConstants.CandleState.MAGENTA
-      fadeBetweenColors(state, otherState)
+      fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2)
+    }
+    else if (state == LEDConstants.CandleState.RED) {
+      ledController.clearAnimation(0)
+      fadeBetweenColors(state, LEDConstants.CandleState.LIGHT_RED, loopCyclesToConverge = 2)
     }
     else if (state.animation == null) {
       ledController.clearAnimation(0)
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
index 495d6841..4ca49ca3 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -63,7 +63,9 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
   init {}
 
   override fun periodic() {
-    io.setPipeline(0)
+    visibleGamePieces.clear()
+    targetGamePieceTx = null
+
     val startTime = Clock.realTimestamp
     io.updateInputs(inputs)
     Logger.processInputs("LimelightVision", inputs)

From 8b4b31f0e6a90e164e4d91c5fa8be5efc4e4b1d9 Mon Sep 17 00:00:00 2001
From: Matthew Choulas <MatthewChoulas@gmail.com>
Date: Mon, 1 Apr 2024 13:22:12 -0400
Subject: [PATCH 4/7] fix feeder and LL

---
 .../com/team4099/robot2023/RobotContainer.kt  |  1 -
 .../config/constants/DrivetrainConstants.kt   |  4 +--
 .../config/constants/VisionConstants.kt       |  4 +--
 .../subsystems/limelight/LimelightVision.kt   |  4 +++
 .../superstructure/Superstructure.kt          | 35 +++++++++++++------
 5 files changed, 32 insertions(+), 16 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 9ccc73d4..d44cd072 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -169,7 +169,6 @@ object RobotContainer {
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
     ControlBoard.intake.whileTrue(
       ParallelCommandGroup(
-        superstructure.groundIntakeCommand(),
         TargetNoteCommand(
           driver = Ryan(),
           { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
index 41ec3a85..f455d768 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
@@ -128,10 +128,10 @@ object DrivetrainConstants {
         }
       }
 
-    val LIMELIGHT_THETA_KP = 0.0.degrees.perSecond / 1.degrees
+    val LIMELIGHT_THETA_KP = 4.0.degrees.perSecond / 1.degrees
     val LIMELIGHT_THETA_KI = 0.0.degrees.perSecond / (1.degrees * 1.seconds)
     val LIMELIGHT_THETA_KD =
-      (0.0.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond
+      (0.1.degrees.perSecond / (1.degrees / 1.seconds)).radiansPerSecondPerRadiansPerSecond
 
     val AUTO_THETA_ALLOWED_ERROR = 3.degrees
     val AUTO_THETA_PID_KP = (1.05.radians.perSecond / 1.radians)
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
index babc80e8..93715b3d 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/VisionConstants.kt
@@ -84,8 +84,8 @@ object VisionConstants {
     val MID_TAPE_HEIGHT = 23.905.inches + 1.inches
     val LL_TRANSFORM =
       Transform3d(
-        Translation3d(1.1438.inches, 10.3966.inches, 12.9284.inches),
-        Rotation3d(8.159.degrees, -90.degrees + 61.610.degrees, -14.1254.degrees)
+        Translation3d(-14.655.inches, 0.inches, 23.316.inches),
+        Rotation3d(0.degrees, 143.degrees, 180.degrees)
       )
     const val RES_WIDTH = 320
     const val RES_HEIGHT = 240 // no clue what these numbers should be but usnig these for now
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
index 4ca49ca3..6338b25a 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -63,6 +63,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
   init {}
 
   override fun periodic() {
+
+
     visibleGamePieces.clear()
     targetGamePieceTx = null
 
@@ -92,6 +94,8 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
       }
     }
 
+    Logger.recordOutput("Limelight/poseRelativeToRobot", ((targetGamePiecePose?.toPose2d() ?: Pose2d()) - poseSupplier.invoke()).transform2d)
+
     Logger.recordOutput(
       "LimelightVision/RawLimelightReadingsTx",
       inputs.gamePieceTargets.map { it.tx.inDegrees }.toDoubleArray()
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index e7635db8..87b4b677 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -398,13 +398,7 @@ class Superstructure(
           )
 
         if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) {
-          if (DriverStation.isTeleop()) {
-            cleanupStartTime = Clock.fpgaTime
-            nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP
-          } else {
-            currentRequest = Request.SuperstructureRequest.Idle()
-            nextState = SuperstructureStates.IDLE
-          }
+            nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK
         }
         when (currentRequest) {
           is Request.SuperstructureRequest.Idle -> {
@@ -421,13 +415,31 @@ class Superstructure(
           }
         }
       }
-      SuperstructureStates.GROUND_INTAKE_CLEAN_UP -> {
-        feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.volts)
+      SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK -> {
+        feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(-1.0.volts)
+        if (!feeder.hasNote) {
+          nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD
+        }
 
-        if (Clock.fpgaTime - cleanupStartTime > FeederConstants.CLEAN_UP_TIME) {
+        when (currentRequest) {
+          is Request.SuperstructureRequest.Idle -> {
+            nextState = SuperstructureStates.IDLE
+          }
+        }
+      }
+      SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD -> {
+        feeder.currentRequest = Request.FeederRequest.OpenLoopIntake(1.volts)
+        if (feeder.hasNote) {
           currentRequest = Request.SuperstructureRequest.Idle()
           nextState = SuperstructureStates.IDLE
         }
+        when (currentRequest) {
+          is Request.SuperstructureRequest.Idle -> {
+            nextState = SuperstructureStates.IDLE
+          }
+        }
+
+
       }
       SuperstructureStates.AUTO_AIM -> {
         val targetFlywheelSpeed = aimer.calculateFlywheelSpeed()
@@ -1022,7 +1034,8 @@ class Superstructure(
       HOME,
       GROUND_INTAKE_PREP,
       GROUND_INTAKE,
-      GROUND_INTAKE_CLEAN_UP,
+      GROUND_INTAKE_CLEAN_UP_PUSH_BACK,
+      GROUND_INTAKE_CLEAN_UP_PUSH_FORWARD,
       ELEVATOR_AMP_PREP,
       WRIST_AMP_PREP,
       SCORE_ELEVATOR_AMP,

From 448b6fc81b30e4da85d816f1aba9a649f6aa02c4 Mon Sep 17 00:00:00 2001
From: Matthew Choulas <MatthewChoulas@gmail.com>
Date: Tue, 2 Apr 2024 02:47:20 -0400
Subject: [PATCH 5/7] finish leds

---
 .../com/team4099/robot2023/RobotContainer.kt  |  3 +-
 .../commands/drivetrain/TargetNoteCommand.kt  |  9 +++-
 .../robot2023/config/constants/Constants.kt   |  5 +--
 .../config/constants/LEDConstants.kt          |  6 +++
 .../team4099/robot2023/subsystems/led/Leds.kt | 43 +++++++++++++------
 .../superstructure/Superstructure.kt          |  8 +++-
 .../robot2023/subsystems/vision/Vision.kt     |  2 +-
 7 files changed, 56 insertions(+), 20 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index d44cd072..4764c9a4 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -85,7 +85,7 @@ object RobotContainer {
       feeder = Feeder(FeederIONeo)
       elevator = Elevator(object : ElevatorIO {})
       flywheel = Flywheel(FlywheelIOTalon)
-      wrist = Wrist(object : WristIO {})
+      wrist = Wrist(WristIOTalon)
     } else {
       // Simulation implementations
       drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim)
@@ -169,6 +169,7 @@ object RobotContainer {
     ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain))
     ControlBoard.intake.whileTrue(
       ParallelCommandGroup(
+        superstructure.groundIntakeCommand(),
         TargetNoteCommand(
           driver = Ryan(),
           { ControlBoard.forward.smoothDeadband(Constants.Joysticks.THROTTLE_DEADBAND) },
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
index 4dadbafc..e3a3df37 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
@@ -11,6 +11,7 @@ import edu.wpi.first.wpilibj.RobotBase
 import edu.wpi.first.wpilibj2.command.Command
 import org.team4099.lib.controller.PIDController
 import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Radian
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
@@ -22,8 +23,10 @@ import org.team4099.lib.units.derived.perDegreePerSecond
 import org.team4099.lib.units.derived.perDegreeSeconds
 import org.team4099.lib.units.derived.radians
 import org.team4099.lib.units.inDegreesPerSecond
+import org.team4099.lib.units.inMetersPerSecond
 import org.team4099.lib.units.perSecond
 import kotlin.math.PI
+import kotlin.math.hypot
 
 class TargetNoteCommand(
   val driver: DriverProfile,
@@ -98,9 +101,12 @@ class TargetNoteCommand(
   override fun initialize() {
     thetaPID.reset()
 
+    /*
     if (thetakP.hasChanged() || thetakI.hasChanged() || thetakD.hasChanged()) {
       thetaPID = PIDController(thetakP.get(), thetakI.get(), thetakD.get())
     }
+
+     */
   }
 
   override fun execute() {
@@ -112,10 +118,11 @@ class TargetNoteCommand(
     DebugLogger.recordDebugOutput("NoteAlignment/error", thetaPID.error.inDegrees)
     DebugLogger.recordDebugOutput("NoteAlignment/thetaFeedback", thetaFeedback.inDegreesPerSecond)
 
+    val driveVector = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode)
     drivetrain.currentRequest =
       Request.DrivetrainRequest.OpenLoop(
         thetaFeedback,
-        driver.driveSpeedClampedSupplier(driveX, driveY, slowMode),
+        Pair(-hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond).meters.perSecond, 0.0.meters.perSecond),
         fieldOriented = false
       )
   }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
index c547889c..bad9aaaa 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
@@ -53,9 +53,8 @@ object Constants {
   }
 
   object Tuning {
-
-    const val TUNING_MODE = true
-    const val DEBUGING_MODE = true
+    const val TUNING_MODE = false
+    const val DEBUGING_MODE = false
     const val SIMULATE_DRIFT = false
     const val DRIFT_CONSTANT = 0.001
 
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
index 1d64cc73..38f41e62 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
@@ -1,6 +1,7 @@
 package com.team4099.robot2023.config.constants
 
 import com.ctre.phoenix.led.Animation
+import com.ctre.phoenix.led.StrobeAnimation
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.derived.volts
 
@@ -14,6 +15,7 @@ object LEDConstants {
     // Gold
     NO_NOTE(null, 0, 0, 0),
     NOTHING(null, 0, 0, 0),
+    GOLD(null, 255, 105, 0),
     RED(null, 255, 0, 0),
     LIGHT_RED(null, 255, 67, 36),
     BLUE(null, 0, 0, 255),
@@ -24,9 +26,13 @@ object LEDConstants {
     // Blue
 
     HAS_NOTE(null, 0, 0, 255),
+    NO_TAG(null, 255, 0, 0),
+    SEES_TAG(null, 255, 105, 0),
+    SEES_NOTE(null, 255, 87, 51),
 
     // Yellow
     BATTERY_DISPLAY(null, 255, 105, 0),
+    LOW_BATTERY_WARNING(StrobeAnimation(67, 36, 255), 0, 0, 0),
     WHITE(null, 255, 255, 255),
 
     // Green
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
index ea14466b..eadee04b 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
@@ -14,7 +14,10 @@ class Leds(val io: LedIO) {
 
   var hasNote = false
   var subsystemsAtPosition = false
-  var isIdle = true
+  var isAutoAiming = false
+  var isAmping = false
+  var seesGamePiece = false
+  var seesTag = true
 
   var state = LEDConstants.CandleState.NO_NOTE
     set(value) {
@@ -26,25 +29,41 @@ class Leds(val io: LedIO) {
     io.updateInputs(inputs)
     if (DriverStation.getAlliance().isEmpty) {
       io.batteryVoltage = RobotController.getBatteryVoltage().volts
-      state = LEDConstants.CandleState.BATTERY_DISPLAY
-    }
-    else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) {
+      state = LEDConstants.CandleState.GOLD
+      if (io.batteryVoltage < 12.3.volts) {
+        state = LEDConstants.CandleState.LOW_BATTERY_WARNING
+      } else {
+        state = LEDConstants.CandleState.GOLD
+      }
+    } else if (DriverStation.isDisabled() && DriverStation.getAlliance().isPresent) {
       if (FMSData.isBlue) {
         state = LEDConstants.CandleState.BLUE
-      } else  {
+      } else {
         state = LEDConstants.CandleState.RED
       }
     } else if (hasNote) {
-      if (subsystemsAtPosition && !isIdle) {
-        state = LEDConstants.CandleState.CAN_SHOOT
-      } else {
-        state = LEDConstants.CandleState.HAS_NOTE
+      if (isAutoAiming) {
+        if (!seesTag) {
+          state = LEDConstants.CandleState.NO_TAG
+        } else if (!subsystemsAtPosition) {
+          state = LEDConstants.CandleState.SEES_TAG
+        } else {
+          state = LEDConstants.CandleState.CAN_SHOOT
+        }
+      } else if (isAmping) {
+        if (subsystemsAtPosition) {
+          state = LEDConstants.CandleState.CAN_SHOOT
+        } else {
+          state = LEDConstants.CandleState.HAS_NOTE
+        }
       }
+    }else if (seesGamePiece) {
+      state = LEDConstants.CandleState.SEES_NOTE
     } else {
       state = LEDConstants.CandleState.NO_NOTE
     }
 
-    Logger.processInputs("LED", inputs)
-    Logger.recordOutput("LED/state", state.name)
-  }
+      Logger.processInputs("LED", inputs)
+      Logger.recordOutput("LED/state", state.name)
+    }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index 87b4b677..b7c82621 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -13,6 +13,7 @@ import com.team4099.robot2023.subsystems.flywheel.Flywheel
 import com.team4099.robot2023.subsystems.intake.Intake
 import com.team4099.robot2023.subsystems.led.LedIOCandle
 import com.team4099.robot2023.subsystems.led.Leds
+import com.team4099.robot2023.subsystems.limelight.LimelightVision
 import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.util.NoteSimulation
@@ -48,7 +49,8 @@ class Superstructure(
   private val wrist: Wrist,
   private val flywheel: Flywheel,
   private val drivetrain: Drivetrain,
-  private val vision: Vision
+  private val vision: Vision,
+  private val limelight: LimelightVision
 ) : SubsystemBase() {
 
   var wristPushDownVoltage = Wrist.TunableWristStates
@@ -140,9 +142,11 @@ class Superstructure(
 
   override fun periodic() {
     leds.hasNote = feeder.hasNote
-    leds.isIdle = currentState == SuperstructureStates.IDLE
+    leds.isAutoAiming = currentState == SuperstructureStates.AUTO_AIM
+    leds.isAmping = (currentState == SuperstructureStates.WRIST_AMP_PREP) || (currentState == SuperstructureStates.ELEVATOR_AMP_PREP)
     leds.subsystemsAtPosition =
       wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition
+    leds.seesGamePiece = limelight.inputs.gamePieceTargets.size > 0
 
     val ledLoopStartTime = Clock.realTimestamp
     leds.periodic()
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt
index b98b9ea8..251217cd 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/vision/Vision.kt
@@ -111,7 +111,7 @@ class Vision(vararg cameras: CameraIO) : SubsystemBase() {
               PhotonUtils.calculateDistanceToTargetMeters(
                 cameraPoses[instance].translation.z.inMeters,
                 57.125.inches.inMeters,
-                23.25.degrees.inRadians,
+                22.42.degrees.inRadians,
                 tag.pitch.degrees.inRadians
               )
                 .meters

From 80b40596a84170d20b449e3b6950125fa306677e Mon Sep 17 00:00:00 2001
From: Matthew Choulas <MatthewChoulas@gmail.com>
Date: Tue, 2 Apr 2024 02:53:01 -0400
Subject: [PATCH 6/7] spotless

---
 .../com/team4099/robot2023/RobotContainer.kt  |   5 +-
 .../commands/drivetrain/TargetNoteCommand.kt  |   7 +-
 .../config/constants/LEDConstants.kt          |   8 +-
 .../subsystems/intake/IntakeIOFalconNEO.kt    |   7 +-
 .../robot2023/subsystems/led/LedIOCandle.kt   | 155 ++++++++++++++----
 .../team4099/robot2023/subsystems/led/Leds.kt |  10 +-
 .../subsystems/limelight/LimelightVision.kt   |   6 +-
 .../superstructure/Superstructure.kt          |  12 +-
 8 files changed, 152 insertions(+), 58 deletions(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index 4764c9a4..defaaa69 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -16,7 +16,6 @@ import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO
 import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2
 import com.team4099.robot2023.subsystems.elevator.Elevator
 import com.team4099.robot2023.subsystems.elevator.ElevatorIO
-import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim
 import com.team4099.robot2023.subsystems.feeder.Feeder
 import com.team4099.robot2023.subsystems.feeder.FeederIONeo
@@ -36,7 +35,6 @@ import com.team4099.robot2023.subsystems.vision.Vision
 import com.team4099.robot2023.subsystems.vision.camera.CameraIO
 import com.team4099.robot2023.subsystems.vision.camera.CameraIOPhotonvision
 import com.team4099.robot2023.subsystems.wrist.Wrist
-import com.team4099.robot2023.subsystems.wrist.WristIO
 import com.team4099.robot2023.subsystems.wrist.WristIOSim
 import com.team4099.robot2023.subsystems.wrist.WristIOTalon
 import com.team4099.robot2023.util.driver.Ryan
@@ -98,7 +96,8 @@ object RobotContainer {
       wrist = Wrist(WristIOSim)
     }
 
-    superstructure = Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision)
+    superstructure =
+      Superstructure(intake, feeder, elevator, wrist, flywheel, drivetrain, vision, limelight)
     vision.setDataInterfaces(
       { drivetrain.fieldTRobot },
       { drivetrain.addVisionData(it) },
diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
index e3a3df37..b22ed663 100644
--- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
+++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetNoteCommand.kt
@@ -122,7 +122,12 @@ class TargetNoteCommand(
     drivetrain.currentRequest =
       Request.DrivetrainRequest.OpenLoop(
         thetaFeedback,
-        Pair(-hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond).meters.perSecond, 0.0.meters.perSecond),
+        Pair(
+          -hypot(driveVector.first.inMetersPerSecond, driveVector.second.inMetersPerSecond)
+            .meters
+            .perSecond,
+          0.0.meters.perSecond
+        ),
         fieldOriented = false
       )
   }
diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
index 38f41e62..aafe3014 100644
--- a/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
+++ b/src/main/kotlin/com/team4099/robot2023/config/constants/LEDConstants.kt
@@ -11,7 +11,13 @@ object LEDConstants {
   val BATTERY_FULL_THRESHOLD = 12.5.volts
   val LED_COUNT = 50
 
-  enum class CandleState(val animation: Animation?, val r: Int, val g: Int, val b: Int, var attachedValue: Any? = null) {
+  enum class CandleState(
+    val animation: Animation?,
+    val r: Int,
+    val g: Int,
+    val b: Int,
+    var attachedValue: Any? = null
+  ) {
     // Gold
     NO_NOTE(null, 0, 0, 0),
     NOTHING(null, 0, 0, 0),
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
index acc1e27c..3bf87ce7 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOFalconNEO.kt
@@ -20,7 +20,6 @@ import com.team4099.robot2023.subsystems.falconspin.Neo
 import org.team4099.lib.units.base.amps
 import org.team4099.lib.units.base.celsius
 import org.team4099.lib.units.base.inAmperes
-import org.team4099.lib.units.base.inSeconds
 import org.team4099.lib.units.ctreAngularMechanismSensor
 import org.team4099.lib.units.derived.ElectricalPotential
 import org.team4099.lib.units.derived.inVolts
@@ -60,8 +59,10 @@ object IntakeIOFalconNEO : IntakeIO {
       IntakeConstants.ROLLER_STATOR_CURRENT_LIMIT.inAmperes
     rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimit =
       IntakeConstants.ROLLER_SUPPLY_CURRENT_LIMIT.inAmperes
-//    rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold = IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds
-//    rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold = IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes
+    //    rollerFalconConfiguration.CurrentLimits.SupplyTimeThreshold =
+    // IntakeConstants.ROLLER_CURRENT_TIME_THRESHOLD.inSeconds
+    //    rollerFalconConfiguration.CurrentLimits.SupplyCurrentThreshold =
+    // IntakeConstants.ROLLER_SUPPLY_TRIGGER_THRESHOLD.inAmperes
     rollerFalconConfiguration.CurrentLimits.StatorCurrentLimitEnable = true
     rollerFalconConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true
     rollerFalconConfiguration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
index 07973082..58b1808d 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/LedIOCandle.kt
@@ -38,8 +38,17 @@ object LedIOCandle : LedIO {
     ledController.setLEDs(state.r, state.g, state.b, 0, waveRuns, lengthOfWave)
 
     // Set outer colors
-    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt())
-    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, waveRuns + lengthOfWave + lengthOfGradient.toInt(), LEDConstants.LED_COUNT - waveRuns - lengthOfWave)
+    ledController.setLEDs(
+      defaultState.r, defaultState.g, defaultState.b, 0, 0, waveRuns - lengthOfGradient.toInt()
+    )
+    ledController.setLEDs(
+      defaultState.r,
+      defaultState.g,
+      defaultState.b,
+      0,
+      waveRuns + lengthOfWave + lengthOfGradient.toInt(),
+      LEDConstants.LED_COUNT - waveRuns - lengthOfWave
+    )
 
     // Set gradient colors
     if (lengthOfGradient > 0.0) {
@@ -56,9 +65,24 @@ object LedIOCandle : LedIO {
 
       for ((step, ledIdx) in (waveRuns..(waveRuns + lengthOfGradient).toInt()).withIndex()) {
         ledController.setLEDs(
-          (defaultState.r + (state.r - defaultState.r) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
-          (defaultState.g + (state.g - defaultState.g) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
-          (defaultState.b + (state.b - defaultState.b) * ((lengthOfGradient - (step + 1)) / lengthOfGradient)).toInt(),
+          (
+            defaultState.r +
+              (state.r - defaultState.r) *
+              ((lengthOfGradient - (step + 1)) / lengthOfGradient)
+            )
+            .toInt(),
+          (
+            defaultState.g +
+              (state.g - defaultState.g) *
+              ((lengthOfGradient - (step + 1)) / lengthOfGradient)
+            )
+            .toInt(),
+          (
+            defaultState.b +
+              (state.b - defaultState.b) *
+              ((lengthOfGradient - (step + 1)) / lengthOfGradient)
+            )
+            .toInt(),
           0,
           ledIdx,
           1
@@ -68,8 +92,7 @@ object LedIOCandle : LedIO {
 
     if (waveRuns >= LEDConstants.LED_COUNT - lengthOfWave) {
       reverseLEDS = true
-    }
-    else if (waveRuns < lengthOfWave){
+    } else if (waveRuns < lengthOfWave) {
       reverseLEDS = false
     }
 
@@ -82,13 +105,38 @@ object LedIOCandle : LedIO {
     lengthOfSolidEnds: Int = 10
   ) {
     ledController.setLEDs(state.r, state.g, state.b, 0, 0, lengthOfSolidEnds)
-    ledController.setLEDs(otherState.r, otherState.g, otherState.b, 0, LEDConstants.LED_COUNT - lengthOfSolidEnds, lengthOfSolidEnds)
+    ledController.setLEDs(
+      otherState.r,
+      otherState.g,
+      otherState.b,
+      0,
+      LEDConstants.LED_COUNT - lengthOfSolidEnds,
+      lengthOfSolidEnds
+    )
 
-    for ((step, ledIdx) in (lengthOfSolidEnds until(LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()) {
+    for (
+      (step, ledIdx) in
+      (lengthOfSolidEnds until (LEDConstants.LED_COUNT - lengthOfSolidEnds)).withIndex()
+    ) {
       ledController.setLEDs(
-        (state.r + (otherState.r - state.r) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
-        (state.g + (otherState.g - state.g) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
-        (state.b + (otherState.b - state.b) * ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))).toInt(),
+        (
+          state.r +
+            (otherState.r - state.r) *
+            ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))
+          )
+          .toInt(),
+        (
+          state.g +
+            (otherState.g - state.g) *
+            ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))
+          )
+          .toInt(),
+        (
+          state.b +
+            (otherState.b - state.b) *
+            ((step + 1) / (LEDConstants.LED_COUNT - lengthOfSolidEnds * 2.0))
+          )
+          .toInt(),
         0,
         ledIdx,
         1
@@ -101,8 +149,17 @@ object LedIOCandle : LedIO {
     defaultState: LEDConstants.CandleState,
     percent: Double
   ) {
-    ledController.setLEDs(state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt())
-    ledController.setLEDs(defaultState.r, defaultState.g, defaultState.b, 0, (percent * LEDConstants.LED_COUNT).toInt(), LEDConstants.LED_COUNT)
+    ledController.setLEDs(
+      state.r, state.g, state.b, 0, 0, (percent * LEDConstants.LED_COUNT).toInt()
+    )
+    ledController.setLEDs(
+      defaultState.r,
+      defaultState.g,
+      defaultState.b,
+      0,
+      (percent * LEDConstants.LED_COUNT).toInt(),
+      LEDConstants.LED_COUNT
+    )
   }
 
   private fun fadeBetweenColors(
@@ -114,22 +171,55 @@ object LedIOCandle : LedIO {
     var reachedStartColor = true
     var reachedEndColor = true
 
-    for ((ledIdx, numberOfLoopCycles) in (loopCyclesToConverge..loopCyclesToConverge + LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles).withIndex()) {
-      val calculatedR = (state.r + (otherState.r - state.r) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
-      val calculatedG = (state.g + (otherState.g - state.g) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
-      val calculatedB = (state.b + (otherState.b - state.b) * (if (loopCycles > numberOfLoopCycles) 1.0 else (loopCycles / numberOfLoopCycles.toDouble()))).toInt()
+    for (
+      (ledIdx, numberOfLoopCycles) in
+      (
+        loopCyclesToConverge..loopCyclesToConverge +
+          LEDConstants.LED_COUNT * stepUpInLoopCycles step stepUpInLoopCycles
+        )
+        .withIndex()
+    ) {
+      val calculatedR =
+        (
+          state.r +
+            (otherState.r - state.r) *
+            (
+              if (loopCycles > numberOfLoopCycles) 1.0
+              else (loopCycles / numberOfLoopCycles.toDouble())
+              )
+          )
+          .toInt()
+      val calculatedG =
+        (
+          state.g +
+            (otherState.g - state.g) *
+            (
+              if (loopCycles > numberOfLoopCycles) 1.0
+              else (loopCycles / numberOfLoopCycles.toDouble())
+              )
+          )
+          .toInt()
+      val calculatedB =
+        (
+          state.b +
+            (otherState.b - state.b) *
+            (
+              if (loopCycles > numberOfLoopCycles) 1.0
+              else (loopCycles / numberOfLoopCycles.toDouble())
+              )
+          )
+          .toInt()
 
-      reachedStartColor = (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b)
-      reachedEndColor = ((calculatedR - otherState.r).absoluteValue < 10 && (calculatedG - otherState.g).absoluteValue < 10 && (calculatedB - otherState.b).absoluteValue < 10)
+      reachedStartColor =
+        (calculatedR == state.r && calculatedG == state.g && calculatedB == state.b)
+      reachedEndColor =
+        (
+          (calculatedR - otherState.r).absoluteValue < 10 &&
+            (calculatedG - otherState.g).absoluteValue < 10 &&
+            (calculatedB - otherState.b).absoluteValue < 10
+          )
 
-      ledController.setLEDs(
-        calculatedR,
-        calculatedG,
-        calculatedB,
-        0,
-        ledIdx,
-        1
-      )
+      ledController.setLEDs(calculatedR, calculatedG, calculatedB, 0, ledIdx, 1)
     }
 
     loopCycles += if (finishedFade) -1 else 1
@@ -150,16 +240,13 @@ object LedIOCandle : LedIO {
         LEDConstants.CandleState.NOTHING,
         (batteryVoltage.inVolts - 11.5) / (LEDConstants.BATTERY_FULL_THRESHOLD.inVolts - 11.5)
       )
-    }
-    else if (state == LEDConstants.CandleState.BLUE) {
+    } else if (state == LEDConstants.CandleState.BLUE) {
       ledController.clearAnimation(0)
       fadeBetweenColors(state, LEDConstants.CandleState.MAGENTA, loopCyclesToConverge = 2)
-    }
-    else if (state == LEDConstants.CandleState.RED) {
+    } else if (state == LEDConstants.CandleState.RED) {
       ledController.clearAnimation(0)
       fadeBetweenColors(state, LEDConstants.CandleState.LIGHT_RED, loopCyclesToConverge = 2)
-    }
-    else if (state.animation == null) {
+    } else if (state.animation == null) {
       ledController.clearAnimation(0)
       ledController.setLEDs(state.r, state.g, state.b)
     } else {
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
index eadee04b..254d2b3a 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/led/Leds.kt
@@ -1,13 +1,11 @@
 package com.team4099.robot2023.subsystems.led
 
-import com.team4099.robot2023.RobotContainer
 import com.team4099.robot2023.config.constants.LEDConstants
 import com.team4099.robot2023.util.FMSData
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotController
 import org.littletonrobotics.junction.Logger
 import org.team4099.lib.units.derived.volts
-import java.sql.Driver
 
 class Leds(val io: LedIO) {
   var inputs = LedIO.LedIOInputs()
@@ -57,13 +55,13 @@ class Leds(val io: LedIO) {
           state = LEDConstants.CandleState.HAS_NOTE
         }
       }
-    }else if (seesGamePiece) {
+    } else if (seesGamePiece) {
       state = LEDConstants.CandleState.SEES_NOTE
     } else {
       state = LEDConstants.CandleState.NO_NOTE
     }
 
-      Logger.processInputs("LED", inputs)
-      Logger.recordOutput("LED/state", state.name)
-    }
+    Logger.processInputs("LED", inputs)
+    Logger.recordOutput("LED/state", state.name)
+  }
 }
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
index 6338b25a..6e9ac8c0 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/limelight/LimelightVision.kt
@@ -64,7 +64,6 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
 
   override fun periodic() {
 
-
     visibleGamePieces.clear()
     targetGamePieceTx = null
 
@@ -94,7 +93,10 @@ class LimelightVision(val io: LimelightVisionIO) : SubsystemBase() {
       }
     }
 
-    Logger.recordOutput("Limelight/poseRelativeToRobot", ((targetGamePiecePose?.toPose2d() ?: Pose2d()) - poseSupplier.invoke()).transform2d)
+    Logger.recordOutput(
+      "Limelight/poseRelativeToRobot",
+      ((targetGamePiecePose?.toPose2d() ?: Pose2d()) - poseSupplier.invoke()).transform2d
+    )
 
     Logger.recordOutput(
       "LimelightVision/RawLimelightReadingsTx",
diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
index b7c82621..219f41ca 100644
--- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
+++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Superstructure.kt
@@ -1,10 +1,8 @@
 package com.team4099.robot2023.subsystems.superstructure
 
 import com.team4099.lib.hal.Clock
-import com.team4099.robot2023.config.constants.FeederConstants
 import com.team4099.robot2023.config.constants.FieldConstants
 import com.team4099.robot2023.config.constants.FlywheelConstants
-import com.team4099.robot2023.config.constants.LEDConstants
 import com.team4099.robot2023.config.constants.WristConstants
 import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain
 import com.team4099.robot2023.subsystems.elevator.Elevator
@@ -19,7 +17,6 @@ import com.team4099.robot2023.subsystems.wrist.Wrist
 import com.team4099.robot2023.util.NoteSimulation
 import edu.wpi.first.wpilibj.DriverStation
 import edu.wpi.first.wpilibj.RobotBase
-import edu.wpi.first.wpilibj.RobotController
 import edu.wpi.first.wpilibj2.command.Command
 import edu.wpi.first.wpilibj2.command.SubsystemBase
 import org.littletonrobotics.junction.Logger
@@ -36,7 +33,6 @@ import org.team4099.lib.units.base.meters
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
-import org.team4099.lib.units.derived.inVolts
 import org.team4099.lib.units.derived.rotations
 import org.team4099.lib.units.derived.volts
 import org.team4099.lib.units.inRotationsPerMinute
@@ -143,7 +139,9 @@ class Superstructure(
   override fun periodic() {
     leds.hasNote = feeder.hasNote
     leds.isAutoAiming = currentState == SuperstructureStates.AUTO_AIM
-    leds.isAmping = (currentState == SuperstructureStates.WRIST_AMP_PREP) || (currentState == SuperstructureStates.ELEVATOR_AMP_PREP)
+    leds.isAmping =
+      (currentState == SuperstructureStates.WRIST_AMP_PREP) ||
+      (currentState == SuperstructureStates.ELEVATOR_AMP_PREP)
     leds.subsystemsAtPosition =
       wrist.isAtTargetedPosition && flywheel.isAtTargetedVelocity && elevator.isAtTargetedPosition
     leds.seesGamePiece = limelight.inputs.gamePieceTargets.size > 0
@@ -402,7 +400,7 @@ class Superstructure(
           )
 
         if (feeder.hasNote || (!RobotBase.isReal() && noteHoldingID != -1)) {
-            nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK
+          nextState = SuperstructureStates.GROUND_INTAKE_CLEAN_UP_PUSH_BACK
         }
         when (currentRequest) {
           is Request.SuperstructureRequest.Idle -> {
@@ -442,8 +440,6 @@ class Superstructure(
             nextState = SuperstructureStates.IDLE
           }
         }
-
-
       }
       SuperstructureStates.AUTO_AIM -> {
         val targetFlywheelSpeed = aimer.calculateFlywheelSpeed()

From 8f3d28defc3252f1ae1560dbfd92a96e0935fdb6 Mon Sep 17 00:00:00 2001
From: Matthew Choulas <MatthewChoulas@gmail.com>
Date: Tue, 2 Apr 2024 15:51:27 -0400
Subject: [PATCH 7/7] elevator real

---
 src/main/kotlin/com/team4099/robot2023/RobotContainer.kt | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
index defaaa69..a4b43c40 100644
--- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
+++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt
@@ -47,6 +47,7 @@ import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import org.team4099.lib.units.derived.inDegrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
+import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -81,7 +82,7 @@ object RobotContainer {
       limelight = LimelightVision(LimelightVisionIOReal)
       intake = Intake(IntakeIOFalconNEO)
       feeder = Feeder(FeederIONeo)
-      elevator = Elevator(object : ElevatorIO {})
+      elevator = Elevator(ElevatorIONEO)
       flywheel = Flywheel(FlywheelIOTalon)
       wrist = Wrist(WristIOTalon)
     } else {