Fix intake issues

src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt

@@ -0,0 +1,100 @@
+package com.team4099.robot2023.subsystems.intake
+
+import com.team4099.lib.hal.Clock
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import com.team4099.robot2023.subsystems.superstructure.Request
+import edu.wpi.first.wpilibj2.command.Command
+import edu.wpi.first.wpilibj2.command.SubsystemBase
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.base.inSeconds
+import org.team4099.lib.units.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+
+class Intake(val io: IntakeIO) : SubsystemBase() {
+    val inputs = IntakeIO.IntakeIOInputs()
+    var rollerVoltageTarget: ElectricalPotential = 0.0.volts
+    var isZeroed = false
+    var currentState: IntakeState = IntakeState.UNINITIALIZED
+
+    var currentRequest: Request.IntakeRequest = Request.IntakeRequest.OpenLoop(IntakeConstants.IDLE_ROLLER_VOLTAGE)
+        set(value) {
+            rollerVoltageTarget = when (value) {
+                is Request.IntakeRequest.OpenLoop -> {
+                    value.rollerVoltage
+                }
+            }
+
+            field = value
+        }
+
+    private var timeProfileGeneratedAt = Clock.fpgaTime
+
+    override fun periodic() {
+        io.updateInputs(inputs)
+
+        Logger.processInputs("GroundIntake", inputs)
+        Logger.recordOutput("GroundIntake/currentState", currentState.name)
+        Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName)
+        Logger.recordOutput("GroundIntake/isZeroed", isZeroed)
+
+        if (Constants.Tuning.DEBUGING_MODE) {
+            Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest))
+            Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds)
+            Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts)
+        }
+
+        var nextState = currentState
+        when (currentState) {
+            IntakeState.UNINITIALIZED -> {
+                // Outputs
+                // No designated output functionality because targeting position will take care of it next
+                // loop cycle
+
+                // Transitions
+                nextState = IntakeState.OPEN_LOOP
+            }
+            IntakeState.OPEN_LOOP -> {
+                setRollerVoltage(rollerVoltageTarget)
+
+                // Transitions
+                nextState = fromRequestToState(currentRequest)
+            }
+        }
+
+        // The next loop cycle, we want to run ground intake at the state that was requested. setting
+        // current state to the next state ensures that we run the logic for the state we want in the
+        // next loop cycle.
+        currentState = nextState
+    }
+
+    /** @param appliedVoltage Represents the applied voltage of the roller motor */
+    fun setRollerVoltage(appliedVoltage: ElectricalPotential) {
+        io.setRollerVoltage(appliedVoltage)
+    }
+
+    fun generateIntakeTestCommand(): Command {
+        val returnCommand = runOnce { currentRequest = Request.IntakeRequest.OpenLoop(12.volts) }
+        return returnCommand
+    }
+
+    companion object {
+        enum class IntakeState {
+            UNINITIALIZED,
+            OPEN_LOOP;
+
+            fun equivalentToRequest(request: Request.IntakeRequest): Boolean {
+                return (
+                        (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP)
+                )
+            }
+        }
+
+        fun fromRequestToState(request: Request.IntakeRequest): IntakeState {
+            return when (request) {
+                is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP
+            }
+        }
+    }
+}

src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt

@@ -0,0 +1,63 @@
+package com.team4099.robot2023.subsystems.intake
+
+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.perMinute
+import org.team4099.lib.units.perSecond
+
+interface IntakeIO {
+    class IntakeIOInputs : LoggableInputs {
+        var rollerVelocity = 0.0.rotations.perMinute
+        var rollerAppliedVoltage = 0.0.volts
+
+        var rollerSupplyCurrent = 0.0.amps
+        var rollerStatorCurrent = 0.0.amps
+
+        var rollerTemp = 0.0.celsius
+
+        var isSimulated = false
+
+        override fun toLog(table: LogTable?) {
+            table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)
+            table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)
+            table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)
+            table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)
+            table?.put("rollerTempCelsius", rollerTemp.inCelsius)
+        }
+
+        override fun fromLog(table: LogTable?) {
+            table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let {
+                rollerVelocity = it.rotations.perSecond
+            }
+
+            table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let {
+                rollerAppliedVoltage = it.volts
+            }
+
+            table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let {
+                rollerSupplyCurrent = it.amps
+            }
+
+            table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let {
+                rollerStatorCurrent = it.amps
+            }
+
+            table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius }
+        }
+    }
+
+    fun updateInputs(io: IntakeIOInputs) {}
+
+    fun setRollerVoltage(voltage: ElectricalPotential) {}
+
+    fun setRollerBrakeMode(brake: Boolean) {}
+}

src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt

@@ -0,0 +1,99 @@
+package com.team4099.robot2023.subsystems.intake
+
+import com.revrobotics.CANSparkMax
+import com.revrobotics.CANSparkMaxLowLevel
+import com.team4099.lib.math.clamp
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import com.team4099.robot2023.subsystems.falconspin.MotorChecker
+import com.team4099.robot2023.subsystems.falconspin.MotorCollection
+import com.team4099.robot2023.subsystems.falconspin.Neo
+import org.littletonrobotics.junction.Logger
+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.derived.ElectricalPotential
+import org.team4099.lib.units.derived.inVolts
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.sparkMaxAngularMechanismSensor
+import kotlin.math.absoluteValue
+
+object IntakeIONEO : IntakeIO {
+    private val rollerSparkMax =
+        CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless)
+
+    private val rollerSensor =
+        sparkMaxAngularMechanismSensor(
+            rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION
+        )
+
+    init {
+        rollerSparkMax.restoreFactoryDefaults()
+        rollerSparkMax.clearFaults()
+
+        rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts)
+        rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt())
+        rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED
+
+        rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
+
+        rollerSparkMax.burnFlash()
+
+        MotorChecker.add(
+            "Ground Intake",
+            "Roller",
+            MotorCollection(
+                mutableListOf(Neo(rollerSparkMax, "Roller Motor")),
+                IntakeConstants.ROLLER_CURRENT_LIMIT,
+                70.celsius,
+                30.amps,
+                90.celsius
+            ),
+        )
+    }
+
+    override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) {
+        inputs.rollerVelocity = rollerSensor.velocity
+        inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput
+        inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps
+
+        // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent
+        // AppliedVoltage = percentOutput * BusVoltage
+        // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent =
+        // percentOutput * statorCurrent
+        inputs.rollerSupplyCurrent =
+            inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue
+        inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius
+
+        Logger.recordOutput(
+            "Intake/rollerMotorOvercurrentFault",
+            rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent)
+        )
+        Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage)
+    }
+
+    /**
+     * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation
+     *
+     * @param voltage the voltage to set the roller motor to
+     */
+    override fun setRollerVoltage(voltage: ElectricalPotential) {
+        rollerSparkMax.setVoltage(
+            clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION)
+                .inVolts
+        )
+    }
+
+    /**
+     * Sets the roller motor brake mode
+     *
+     * @param brake if it brakes
+     */
+    override fun setRollerBrakeMode(brake: Boolean) {
+        if (brake) {
+            rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake
+        } else {
+            rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast
+        }
+    }
+}

src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt

@@ -0,0 +1,53 @@
+package com.team4099.robot2023.subsystems.intake
+
+import com.team4099.lib.math.clamp
+import com.team4099.robot2023.config.constants.Constants
+import com.team4099.robot2023.config.constants.IntakeConstants
+import edu.wpi.first.math.system.plant.DCMotor
+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.inVolts
+import org.team4099.lib.units.derived.rotations
+import org.team4099.lib.units.derived.volts
+import org.team4099.lib.units.perMinute
+
+object IntakeIOSim : IntakeIO {
+    private val rollerSim: FlywheelSim = FlywheelSim(
+        DCMotor.getNEO(1),
+        IntakeConstants.ROLLER_GEAR_RATIO,
+        IntakeConstants.ROLLER_INERTIA
+    )
+
+    private var appliedVoltage = 0.volts;
+    init{}
+
+    override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) {
+
+        rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
+
+        inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute
+        inputs.rollerAppliedVoltage = appliedVoltage
+        inputs.rollerSupplyCurrent = 0.amps
+        inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps
+        inputs.rollerTemp = 0.0.celsius
+
+        inputs.isSimulated = true
+    }
+
+    override fun setRollerVoltage(voltage: ElectricalPotential) {
+        appliedVoltage = voltage
+        rollerSim.setInputVoltage(
+            clamp(
+                voltage,
+                -IntakeConstants.VOLTAGE_COMPENSATION,
+                IntakeConstants.VOLTAGE_COMPENSATION
+            )
+                .inVolts
+        )
+    }
+
+    override fun setRollerBrakeMode(brake: Boolean) {}
+}

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

@@ -60,7 +60,6 @@ sealed interface Request {
 
   sealed interface IntakeRequest : Request {
     class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest
-    class Idle() : IntakeRequest
   }
 
   sealed interface DrivetrainRequest : Request {