Chandana Aradhya Indexer PR

src/main/kotlin/com/team4099/robot2026/config/constants/Constants.kt

@@ -76,6 +76,17 @@ object Constants {
     val TABS = arrayOf("Pre-match", "In-match")
   }
 
+  object Feeder {
+    const val FEEDER_MOTOR_ID = 51
+  }
+
+  object Indexer {
+    const val FLOOR_INDEXER_MOTOR_ID = 50
+    const val SIDE_ROLLER_INDEXER_MOTOR_ID = 52
+    const val TOP_BELT_INDEXER_MOTOR_ID = 53
+    const val BOTTOM_BELT_INDEXER_MOTOR_ID = 54
+  }
+
   object LEDS {
     const val CANDLE_ID = 1
   }

src/main/kotlin/com/team4099/robot2026/config/constants/FeederConstants.kt

@@ -0,0 +1,66 @@
+package com.team4099.robot2026.config.constants
+
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.Ampere
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.grams
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.AccelerationFeedforward
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.StaticFeedforward
+import org.team4099.lib.units.derived.VelocityFeedforward
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.meterSquared
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.kilo
+import org.team4099.lib.units.perMinute
+import org.team4099.lib.units.perSecond
+
+object FeederConstants {
+  const val GEAR_RATIO: Double = 18.0 / 28.0
+
+  val STATOR_CURRENT_LIMIT = 40.0.amps
+  val SUPPLY_CURRENT_LIMIT = 40.0.amps
+
+  val VOLTAGE_COMPENSATION = 12.0.volts
+  val MOMENT_OF_INERTIA = 0.000011.kilo.grams.meterSquared
+  val MAX_ACCELERATION: AngularAcceleration = 0.rotations.perSecond.perSecond
+  val MAX_VELOCITY: AngularVelocity = 0.rotations.perSecond
+  val INDEXER_TOLERANCE = 0.0.rotations.perMinute
+
+  object VELOCITIES {
+    val IDLE_VELOCITY = 0.0.rotations.perSecond
+    val SCORE_VELOCITY = 0.0.rotations.perSecond
+  }
+
+  object PID {
+    val REAL_KP: ProportionalGain<Velocity<Radian>, Ampere> = 0.0.amps / 0.0.radians.perSecond
+    val REAL_KI: IntegralGain<Velocity<Radian>, Ampere> =
+        0.0.amps / (0.0.radians.perSecond * 0.0.seconds)
+    val REAL_KD: DerivativeGain<Velocity<Radian>, Ampere> =
+        0.0.amps / (0.0.radians.perSecond / 0.0.seconds)
+
+    val REAL_KS: StaticFeedforward<Ampere> = 0.0.amps
+    val REAL_KV: VelocityFeedforward<Radian, Ampere> = 0.0.amps / 0.0.radians.perSecond
+    val REAL_KA: AccelerationFeedforward<Radian, Ampere> =
+        0.0.amps / 0.0.radians.perSecond.perSecond
+
+    val SIM_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.0.volts / 0.0.degrees.perSecond
+    val SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
+        0.0.volts / (0.0.rotations.perMinute * 0.0.seconds)
+    val SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
+        0.0.volts / (0.0.rotations.perMinute.perSecond)
+
+    val SIM_KS: StaticFeedforward<Volt> = 0.0.volts
+    val SIM_KV: VelocityFeedforward<Radian, Volt> = 0.0.volts / 0.0.degrees.perSecond
+    val SIM_KA: AccelerationFeedforward<Radian, Volt> = 0.0.volts / 0.0.degrees.perSecond.perSecond
+  }
+}

src/main/kotlin/com/team4099/robot2026/config/constants/IndexerConstants.kt

@@ -0,0 +1,94 @@
+package com.team4099.robot2026.config.constants
+
+import org.team4099.lib.units.AngularAcceleration
+import org.team4099.lib.units.AngularVelocity
+import org.team4099.lib.units.Velocity
+import org.team4099.lib.units.base.Ampere
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.grams
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.AccelerationFeedforward
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Radian
+import org.team4099.lib.units.derived.StaticFeedforward
+import org.team4099.lib.units.derived.VelocityFeedforward
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.meterSquared
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.kilo
+import org.team4099.lib.units.perMinute
+import org.team4099.lib.units.perSecond
+
+object IndexerConstants {
+  object FloorConstants {
+    const val TOP_GEAR_RATIO: Double = (16.0 / 58.0) * (28.0 / 30.0)
+    const val BOTTOM_GEAR_RATIO: Double = 16.0 / 58.0
+
+    val STATOR_CURRENT_LIMIT = 40.0.amps
+    val SUPPLY_CURRENT_LIMIT = 40.0.amps
+
+    val VOLTAGE_COMPENSATION = 12.0.volts
+    val TOP_MOMENT_OF_INERTIA = 0.000015.kilo.grams.meterSquared
+    val BOTTOM_MOMENT_OF_INERTIA = 0.000030.kilo.grams.meterSquared
+    val MAX_ACCELERATION: AngularAcceleration = 0.rotations.perSecond.perSecond
+    val MAX_VELOCITY: AngularVelocity = 0.rotations.perSecond
+    val INDEXER_TOLERANCE = 0.0.rotations.perMinute
+  }
+
+  object BeltConstants {
+    const val GEAR_RATIO: Double = 16.0 / 58.0
+
+    val STATOR_CURRENT_LIMIT = 40.0.amps
+    val SUPPLY_CURRENT_LIMIT = 40.0.amps
+
+    val VOLTAGE_COMPENSATION = 12.0.volts
+    val TOP_MOMENT_OF_INERTIA = 0.0000057.kilo.grams.meterSquared
+    val BOTTOM_MOMENT_OF_INERTIA = 0.0000065.kilo.grams.meterSquared
+    val MAX_ACCELERATION: AngularAcceleration = 0.rotations.perSecond.perSecond
+    val MAX_VELOCITY: AngularVelocity = 0.rotations.perSecond
+    val INDEXER_TOLERANCE = 0.0.rotations.perMinute
+  }
+
+  object SideRollerConstants {
+    const val GEAR_RATIO: Double = 16.0 / 40.0
+
+    val STATOR_CURRENT_LIMIT = 40.0.amps
+    val SUPPLY_CURRENT_LIMIT = 40.0.amps
+
+    val VOLTAGE_COMPENSATION = 12.0.volts
+    val MOMENT_OF_INERTIA = 0.0000050.kilo.grams.meterSquared
+    val MAX_ACCELERATION: AngularAcceleration = 0.rotations.perSecond.perSecond
+    val MAX_VELOCITY: AngularVelocity = 0.rotations.perSecond
+    val INDEXER_TOLERANCE = 0.0.rotations.perMinute
+  }
+
+  val IDLE_VELOCITY = 0.0.rotations.perSecond
+
+  object PID {
+    val REAL_KP: ProportionalGain<Velocity<Radian>, Ampere> = 0.0.amps / 0.0.radians.perSecond
+    val REAL_KI: IntegralGain<Velocity<Radian>, Ampere> =
+        0.0.amps / (0.0.radians.perSecond * 0.0.seconds)
+    val REAL_KD: DerivativeGain<Velocity<Radian>, Ampere> =
+        0.0.amps / (0.0.radians.perSecond / 0.0.seconds)
+
+    val REAL_KS: StaticFeedforward<Ampere> = 0.0.amps
+    val REAL_KV: VelocityFeedforward<Radian, Ampere> = 0.0.amps / 0.0.radians.perSecond
+    val REAL_KA: AccelerationFeedforward<Radian, Ampere> =
+        0.0.amps / 0.0.radians.perSecond.perSecond
+
+    val SIM_KP: ProportionalGain<Velocity<Radian>, Volt> = 0.0.volts / 0.0.degrees.perSecond
+    val SIM_KI: IntegralGain<Velocity<Radian>, Volt> =
+        0.0.volts / (0.0.rotations.perMinute * 0.0.seconds)
+    val SIM_KD: DerivativeGain<Velocity<Radian>, Volt> =
+        0.0.volts / (0.0.rotations.perMinute.perSecond)
+
+    val SIM_KS: StaticFeedforward<Volt> = 0.0.volts
+    val SIM_KV: VelocityFeedforward<Radian, Volt> = 0.0.volts / 0.0.degrees.perSecond
+    val SIM_KA: AccelerationFeedforward<Radian, Volt> = 0.0.volts / 0.0.degrees.perSecond.perSecond
+  }
+}

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/Request.kt

@@ -6,6 +6,18 @@ import org.team4099.lib.units.derived.ElectricalPotential
 sealed interface Request {
   sealed interface SuperstructureRequest : Request {}
 
+  sealed interface IndexerRequest : Request {
+    class Idle() : IndexerRequest
+
+    class OpenLoop(val voltage: ElectricalPotential) : IndexerRequest
+  }
+
+  sealed interface FeederRequest : Request {
+    class Idle() : FeederRequest
+
+    class OpenLoop(val voltage: ElectricalPotential) : FeederRequest
+  }
+
   sealed interface IntakeRollerRequest : Request {
     class Idle() : IntakeRollerRequest
 

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/feeder/Feeder.kt

@@ -0,0 +1,63 @@
+package com.team4099.robot2026.subsystems.superstructure.feeder
+
+import com.team4099.robot2026.subsystems.superstructure.Request.FeederRequest
+import com.team4099.robot2026.util.ControlledByStateMachine
+import com.team4099.robot2026.util.CustomLogger
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+
+class Feeder(private val io: FeederIO) : ControlledByStateMachine() {
+  val inputs = FeederIO.FeederInputs()
+
+  var targetVoltage = 0.0.volts
+    private set
+
+  var currentState: FeederState = FeederState.UNINITIALIZED
+  var currentRequest: FeederRequest = FeederRequest.Idle()
+    set(value) {
+      when (value) {
+        is FeederRequest.OpenLoop -> targetVoltage = value.voltage
+        else -> {}
+      }
+    }
+
+  override fun onLoop() {
+    io.updateInputs(inputs)
+    CustomLogger.processInputs("Feeder", inputs)
+
+    CustomLogger.recordOutput("Feeder/currentState", currentState)
+    CustomLogger.recordOutput("Feeder/currentRequest", currentRequest.javaClass.simpleName)
+
+    CustomLogger.recordOutput("Feeder/targetVoltageVolts", targetVoltage.inVolts)
+
+    var nextState = currentState
+    when (currentState) {
+      FeederState.UNINITIALIZED -> {
+        nextState = fromRequestToState(currentRequest)
+      }
+      FeederState.IDLE -> {
+        nextState = fromRequestToState(currentRequest)
+      }
+      FeederState.OPEN_LOOP -> {
+        io.setVoltage(targetVoltage)
+        nextState = fromRequestToState(currentRequest)
+      }
+    }
+    currentState = nextState
+  }
+
+  companion object {
+    enum class FeederState {
+      UNINITIALIZED,
+      IDLE,
+      OPEN_LOOP
+    }
+
+    inline fun fromRequestToState(request: FeederRequest): FeederState {
+      return when (request) {
+        is FeederRequest.Idle -> FeederState.IDLE
+        is FeederRequest.OpenLoop -> FeederState.OPEN_LOOP
+      }
+    }
+  }
+}

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/feeder/FeederIO.kt

@@ -0,0 +1,62 @@
+package com.team4099.robot2026.subsystems.superstructure.feeder
+
+import org.littletonrobotics.junction.LogTable
+import org.littletonrobotics.junction.inputs.LoggableInputs
+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.inCelsius
+import org.team4099.lib.units.derived.ElectricalPotential
+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
+import org.team4099.lib.units.inRotationsPerMinutePerMinute
+import org.team4099.lib.units.perMinute
+
+interface FeederIO {
+  class FeederInputs : LoggableInputs {
+    var feederVelocity = 0.0.rotations.perMinute
+    var feederAcceleration = 0.0.rotations.perMinute.perMinute
+    var feederTemperature = 0.0.celsius
+    var feederAppliedVoltage = 0.0.volts
+    var feederSupplyCurrent = 0.0.amps
+    var feederStatorCurrent = 0.0.amps
+
+    override fun toLog(table: LogTable) {
+      table.put("feederVelocityRPM", feederVelocity.inRotationsPerMinute)
+      table.put("feederAccelerationRPMPM", feederAcceleration.inRotationsPerMinutePerMinute)
+      table.put("feederSupplyCurrentAmps", feederSupplyCurrent.inAmperes)
+      table.put("feederStatorCurrentAmps", feederStatorCurrent.inAmperes)
+      table.put("feederAppliedVoltageVolts", feederAppliedVoltage.inVolts)
+      table.put("feederTemperatureCelsius", feederTemperature.inCelsius)
+    }
+
+    override fun fromLog(table: LogTable) {
+      table.get("feederVelocityRPM", feederVelocity.inRotationsPerMinute).let {
+        feederVelocity = it.rotations.perMinute
+      }
+      table.get("feederAccelerationRPMPM", feederAcceleration.inRotationsPerMinutePerMinute).let {
+        feederAcceleration = it.rotations.perMinute.perMinute
+      }
+      table.get("feederSupplyCurrentAmps", feederSupplyCurrent.inAmperes).let {
+        feederSupplyCurrent = it.amps
+      }
+      table.get("feederStatorCurrentAmps", feederStatorCurrent.inAmperes).let {
+        feederStatorCurrent = it.amps
+      }
+      table.get("feederAppliedVoltageVolts", feederAppliedVoltage.inVolts).let {
+        feederAppliedVoltage = it.volts
+      }
+      table.get("feederTemperatureCelsius", feederTemperature.inCelsius).let {
+        feederTemperature = it.celsius
+      }
+    }
+  }
+
+  fun updateInputs(inputs: FeederInputs) {}
+
+  fun setVoltage(voltage: ElectricalPotential) {}
+
+  fun setBrakeMode(brake: Boolean) {}
+}

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/feeder/FeederIOSim.kt

@@ -0,0 +1,47 @@
+package com.team4099.robot2026.subsystems.superstructure.feeder
+
+import com.team4099.lib.math.clamp
+import com.team4099.robot2026.config.constants.Constants
+import com.team4099.robot2026.config.constants.FeederConstants
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.math.system.plant.LinearSystemId
+import edu.wpi.first.wpilibj.simulation.FlywheelSim
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.celsius
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inKilogramsMeterSquared
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.radians
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perSecond
+
+object FeederIOSim : FeederIO {
+  private val feederSim =
+      FlywheelSim(
+          LinearSystemId.createFlywheelSystem(
+              DCMotor.getKrakenX60Foc(1),
+              FeederConstants.MOMENT_OF_INERTIA.inKilogramsMeterSquared,
+              1.0 / FeederConstants.GEAR_RATIO,
+          ),
+          DCMotor.getKrakenX60Foc(1))
+
+  private var appliedVoltage = 0.0.volts
+
+  override fun updateInputs(inputs: FeederIO.FeederInputs) {
+    feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+    inputs.feederVelocity = feederSim.angularVelocityRadPerSec.radians.perSecond
+    inputs.feederAcceleration = feederSim.angularAccelerationRadPerSecSq.radians.perSecond.perSecond
+    inputs.feederAppliedVoltage = appliedVoltage
+    inputs.feederSupplyCurrent = 0.0.amps
+    inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps
+    inputs.feederTemperature = 0.0.celsius
+  }
+
+  override fun setVoltage(voltage: ElectricalPotential) {
+    val clampedVoltage =
+        clamp(voltage, -FeederConstants.VOLTAGE_COMPENSATION, FeederConstants.VOLTAGE_COMPENSATION)
+    feederSim.setInputVoltage(clampedVoltage.inVolts)
+    appliedVoltage = clampedVoltage
+  }
+}