Justin lau intake

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

@@ -12,6 +12,14 @@ import org.team4099.lib.units.milli
 import org.team4099.lib.units.perSecond
 
 object Constants {
+  object Lintake {
+    val LINTAKE_MOTOR_ID = 0
+  }
+
+  object Rollers {
+    val ROLLERS_MOTOR_ID = 1
+  }
+
   object Universal {
     val gravity = 9.81.meters.perSecond.perSecond
     val SIM_MODE = Tuning.SimType.SIM

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

@@ -0,0 +1,55 @@
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.grams
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.base.seconds
+import org.team4099.lib.units.derived.meterSquared
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.kilo
+import org.team4099.lib.units.milli
+import org.team4099.lib.units.perSecond
+
+object IntakeConstants {
+  object LintakePID {
+    val SIM_KP = 0.0.volts / 1.inches
+    val SIM_KI = 0.0.volts / (1.inches * 1.seconds)
+    val SIM_KD = 0.0.volts / (1.inches.perSecond)
+
+    val REAL_KP = 0.0.volts / 1.inches
+    val REAL_KI = 0.0.volts / (1.inches * 1.seconds)
+    val REAL_KD = 0.0.volts / (1.inches.perSecond)
+
+    val SIM_KS = 0.0.volts
+
+    val REAL_KS = 0.0.volts
+  }
+
+  object LinearIntakeConstants {
+    val STATOR_CURRENT_LIMIT = 40.0.amps
+    val SUPPLY_CURRENT_LIMIT = 40.0.amps
+    val IDLE_VOLTAGE = 0.0.volts
+    val GEAR_RATIO: Double = (1.0 / 4.0) * (18.0 / 30.0) * (10.0 / 27.0)
+    val VOLTAGE_COMPENSATION = 12.0.volts
+    val DIAMETER = (38.89375 * 2.0).milli.meters
+    val MAX_ACCELERATION = 0.0.inches.perSecond.perSecond
+    val MAX_VELOCITY = 0.0.inches.perSecond
+    val LINTAKE_MASS = 6803.89.grams
+  }
+
+  object LintakePosConstants {
+
+    val FORWARD_EXTENSION_LIM = 14.5.inches
+    val BACKMOST_EXTENSION_LIM = 0.0.inches
+    val POSITION_TOLERANCE = 0.25.inches
+    val START_POSITION = 0.0.inches
+  }
+
+  object RollerIntakeConstants {
+    val IDLE_VOLTAGE = 0.0.volts
+    val STATOR_CURRENT_LIMIT = 40.0.amps
+    val SUPPLY_CURRENT_LIMIT = 40.0.amps
+    val GEAR_RATIO = (16.0 / 58.0) * (58.0 / 34.0) * (34.0 / 48.0)
+    val VOLTAGE_COMPENSATION = 12.0.volts
+    val MOMENT_OF_INERTIA = 0.0001.kilo.grams.meterSquared
+  }
+}

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

@@ -1,5 +1,22 @@
 package com.team4099.robot2026.subsystems.superstructure
 
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.derived.ElectricalPotential
+
 sealed interface Request {
   sealed interface SuperstructureRequest : Request {}
+
+  sealed interface IntakeRollerRequest : Request {
+    class Idle() : IntakeRollerRequest
+
+    class OpenLoop(val voltage: ElectricalPotential) : IntakeRollerRequest
+  }
+
+  sealed interface LintakeRequest : Request {
+    class Idle() : LintakeRequest
+
+    class OpenLoop(val voltage: ElectricalPotential) : LintakeRequest
+
+    class ClosedLoop(val position: Length) : LintakeRequest
+  }
 }

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/intake/LinearIntake.kt

@@ -0,0 +1,100 @@
+package com.team4099.robot2026.subsystems.superstructure.intake
+
+import com.team4099.robot2026.subsystems.superstructure.Request
+import com.team4099.robot2026.util.ControlledByStateMachine
+import com.team4099.robot2026.util.CustomLogger
+import edu.wpi.first.wpilibj.RobotBase
+import org.team4099.lib.units.base.inInches
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+
+class LinearIntake(private val io: LinearIntakeIO) : ControlledByStateMachine() {
+  val inputs = LinearIntakeIO.LintakeIOInputs()
+  var currentState = LintakeStates.UNITITALIZED
+  var currentRequest: Request.LintakeRequest = Request.LintakeRequest.Idle()
+    set(value) {
+      when (value) {
+        is Request.LintakeRequest.Idle ->
+            targetVoltage = IntakeConstants.LinearIntakeConstants.IDLE_VOLTAGE
+        is Request.LintakeRequest.OpenLoop -> targetVoltage = value.voltage
+        is Request.LintakeRequest.ClosedLoop -> targetPosition = value.position
+      }
+      field = value
+    }
+
+  var targetVoltage = 0.0.volts
+    private set
+
+  var targetPosition = IntakeConstants.LintakePosConstants.START_POSITION
+    private set
+
+  val isAtTargetPosition: Boolean
+    get() =
+        (currentRequest is Request.LintakeRequest.ClosedLoop &&
+            (inputs.lintakePosition - targetPosition).absoluteValue <=
+                IntakeConstants.LintakePosConstants.POSITION_TOLERANCE)
+
+  init {
+    if (RobotBase.isReal()) {
+      io.configPID(
+          IntakeConstants.LintakePID.REAL_KP,
+          IntakeConstants.LintakePID.REAL_KI,
+          IntakeConstants.LintakePID.REAL_KD)
+      io.configFF(IntakeConstants.LintakePID.REAL_KS)
+    } else {
+      io.configPID(
+          IntakeConstants.LintakePID.SIM_KP,
+          IntakeConstants.LintakePID.SIM_KI,
+          IntakeConstants.LintakePID.SIM_KD)
+      io.configFF(IntakeConstants.LintakePID.SIM_KS)
+    }
+  }
+
+  override fun onLoop() {
+
+    io.updateInputs(inputs)
+    CustomLogger.processInputs("Lintake", inputs)
+    CustomLogger.recordOutput("Lintake/CurrentState", currentState.name)
+    CustomLogger.recordOutput("Lintake/CurrentRequest", currentRequest.javaClass.simpleName)
+    CustomLogger.recordOutput("Lintake/TargetVoltage", targetVoltage.inVolts)
+    CustomLogger.recordOutput("Lintake/TargetPosition", targetPosition.inInches)
+    CustomLogger.recordOutput("Lintake/isAtTargetPosition", isAtTargetPosition)
+
+    var nextState = currentState
+    when (currentState) {
+      LintakeStates.UNITITALIZED -> {
+        nextState = LinearIntake.Companion.fromRequestToState(currentRequest)
+      }
+      LintakeStates.OPEN_LOOP -> {
+        io.setVoltage(targetVoltage)
+        nextState = LinearIntake.Companion.fromRequestToState(currentRequest)
+      }
+      LintakeStates.IDLE -> {
+        io.setVoltage(IntakeConstants.LinearIntakeConstants.IDLE_VOLTAGE)
+        nextState = LinearIntake.Companion.fromRequestToState(currentRequest)
+      }
+      LintakeStates.CLOSED_LOOP -> {
+        io.setPosition(targetPosition)
+        nextState = LinearIntake.Companion.fromRequestToState(currentRequest)
+      }
+    }
+    currentState = nextState
+  }
+
+  companion object {
+    enum class LintakeStates {
+      OPEN_LOOP,
+      IDLE,
+      UNITITALIZED,
+      CLOSED_LOOP
+    }
+
+    inline fun fromRequestToState(request: Request.LintakeRequest): LintakeStates {
+      return when (request) {
+        is Request.LintakeRequest.Idle -> LintakeStates.IDLE
+        is Request.LintakeRequest.OpenLoop -> LintakeStates.OPEN_LOOP
+        is Request.LintakeRequest.ClosedLoop -> LintakeStates.CLOSED_LOOP
+      }
+    }
+  }
+}

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/intake/LinearIntakeIO.kt

@@ -0,0 +1,84 @@
+package com.team4099.robot2026.subsystems.superstructure.intake
+
+import org.littletonrobotics.junction.LogTable
+import org.littletonrobotics.junction.inputs.LoggableInputs
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.Meter
+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.base.inInches
+import org.team4099.lib.units.base.inches
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.StaticFeedforward
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.inInchesPerSecond
+import org.team4099.lib.units.perSecond
+
+interface LinearIntakeIO {
+
+  class LintakeIOInputs : LoggableInputs {
+    var lintakeTemperature = 0.0.celsius
+    var lintakeStatorCurrent = 0.amps
+    var lintakeSupplyCurrent = 0.amps
+    var lintakePosition = 0.inches
+    var lintakeVelocity = 0.inches.perSecond
+    var lintakeAppliedVoltage = 0.volts
+
+    override fun toLog(table: LogTable?) {
+      table?.put("lintakeTemperatureCelsius", lintakeTemperature.inCelsius)
+      table?.put("lintakeStatorCurrentAmps", lintakeStatorCurrent.inAmperes)
+      table?.put("lintakeSupplyCurrentAmps", lintakeSupplyCurrent.inAmperes)
+      table?.put("lintakePositionInches", lintakePosition.inInches)
+      table?.put("lintakeVelocityInchesPerSec", lintakeVelocity.inInchesPerSecond)
+      table?.put("lintakeAppliedVolts", lintakeAppliedVoltage.inVolts)
+    }
+
+    override fun fromLog(table: LogTable?) {
+      table?.get("lintakeTemperatureCelsius", lintakeTemperature.inCelsius)?.let {
+        lintakeTemperature = it.celsius
+      }
+      table?.get("lintakeStatorCurrentAmps", lintakeStatorCurrent.inAmperes)?.let {
+        lintakeStatorCurrent = it.amps
+      }
+      table?.get("lintakeSupplyCurrentAmps", lintakeSupplyCurrent.inAmperes)?.let {
+        lintakeSupplyCurrent = it.amps
+      }
+      table?.get("lintakeAppliedVolts", lintakeAppliedVoltage.inVolts)?.let {
+        lintakeAppliedVoltage = it.volts
+      }
+      table?.get("lintakePositionInches", lintakePosition.inInches)?.let {
+        lintakePosition = it.inches
+      }
+      table?.get("lintakeVelocityInchesPerSec", lintakeVelocity.inInchesPerSecond)?.let {
+        lintakeVelocity = it.inches.perSecond
+      }
+    }
+  }
+
+  fun updateInputs(inputs: LintakeIOInputs) {}
+
+  fun setVoltage(voltage: ElectricalPotential) {}
+
+  fun setPosition(position: Length) {}
+
+  fun configPID(
+      kP: ProportionalGain<Meter, Volt>,
+      kI: IntegralGain<Meter, Volt>,
+      kD: DerivativeGain<Meter, Volt>
+  ) {}
+
+  fun configFF(
+      kS: StaticFeedforward<Volt>,
+  ) {}
+
+  fun setBrakeMode(brake: Boolean) {}
+
+  fun zeroEncoder() {}
+}

src/main/kotlin/com/team4099/robot2026/subsystems/superstructure/intake/LinearIntakeIOSim.kt

@@ -0,0 +1,90 @@
+package com.team4099.robot2026.subsystems.superstructure.intake
+
+import com.team4099.lib.math.clamp
+import com.team4099.robot2026.config.constants.Constants
+import edu.wpi.first.math.system.plant.DCMotor
+import edu.wpi.first.wpilibj.simulation.BatterySim
+import edu.wpi.first.wpilibj.simulation.ElevatorSim
+import edu.wpi.first.wpilibj.simulation.RoboRioSim
+import org.team4099.lib.controller.ProfiledPIDController
+import org.team4099.lib.controller.TrapezoidProfile
+import org.team4099.lib.units.base.Length
+import org.team4099.lib.units.base.Meter
+import org.team4099.lib.units.base.amps
+import org.team4099.lib.units.base.celsius
+import org.team4099.lib.units.base.inGrams
+import org.team4099.lib.units.base.inMeters
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.base.meters
+import org.team4099.lib.units.derived.DerivativeGain
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.IntegralGain
+import org.team4099.lib.units.derived.ProportionalGain
+import org.team4099.lib.units.derived.Volt
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perSecond
+
+object LinearIntakeIOSim : LinearIntakeIO {
+  private val lintakeSim: ElevatorSim =
+      ElevatorSim(
+          DCMotor.getKrakenX60(1),
+          1.0 / IntakeConstants.LinearIntakeConstants.GEAR_RATIO,
+          IntakeConstants.LinearIntakeConstants.LINTAKE_MASS.inGrams,
+          IntakeConstants.LinearIntakeConstants.DIAMETER.inMeters / 2,
+          IntakeConstants.LintakePosConstants.FORWARD_EXTENSION_LIM.inMeters,
+          IntakeConstants.LintakePosConstants.BACKMOST_EXTENSION_LIM.inMeters,
+          false,
+          IntakeConstants.LintakePosConstants.START_POSITION.inMeters)
+
+  private var lastAppliedVoltage = 0.0.volts
+  private var lintakePIDController =
+      ProfiledPIDController(
+          IntakeConstants.LintakePID.SIM_KP,
+          IntakeConstants.LintakePID.SIM_KI,
+          IntakeConstants.LintakePID.SIM_KD,
+          TrapezoidProfile.Constraints(
+              IntakeConstants.LinearIntakeConstants.MAX_VELOCITY,
+              IntakeConstants.LinearIntakeConstants.MAX_ACCELERATION,
+          ))
+
+  override fun updateInputs(inputs: LinearIntakeIO.LintakeIOInputs) {
+    lintakeSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+    inputs.lintakePosition = lintakeSim.positionMeters.meters
+    inputs.lintakeTemperature = 0.0.celsius
+    inputs.lintakeStatorCurrent = lintakeSim.currentDrawAmps.amps
+    inputs.lintakeSupplyCurrent = 0.0.amps
+    inputs.lintakeAppliedVoltage = lastAppliedVoltage
+    inputs.lintakeVelocity = lintakeSim.velocityMetersPerSecond.meters.perSecond
+    RoboRioSim.setVInVoltage(
+        BatterySim.calculateDefaultBatteryLoadedVoltage(lintakeSim.currentDrawAmps))
+  }
+
+  override fun setVoltage(targetVoltage: ElectricalPotential) {
+    val clampedVoltage =
+        clamp(
+            targetVoltage,
+            -IntakeConstants.LinearIntakeConstants.VOLTAGE_COMPENSATION,
+            IntakeConstants.LinearIntakeConstants.VOLTAGE_COMPENSATION)
+    lastAppliedVoltage = clampedVoltage
+    lintakeSim.setInputVoltage(clampedVoltage.inVolts)
+  }
+
+  override fun setPosition(position: Length) {
+    lintakePIDController.setGoal(position)
+    val pidOutput = lintakePIDController.calculate(lintakeSim.positionMeters.meters)
+    setVoltage(pidOutput)
+  }
+
+  override fun zeroEncoder() {
+    lintakeSim.setState(0.0, 0.0)
+  }
+
+  override fun configPID(
+      kP: ProportionalGain<Meter, Volt>,
+      kI: IntegralGain<Meter, Volt>,
+      kD: DerivativeGain<Meter, Volt>
+  ) {
+    lintakePIDController.setPID(kP, kI, kD)
+  }
+}