diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt index 362970ed..9bd6e98d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -17,12 +17,19 @@ import org.team4099.lib.units.perSecond object FeederConstants { val FEEDER_INIT_VOLTAGE = 0.0.volts + // TODO: Add value to Feeder target velocity + val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute + // TODO: Tune PID variables val FEEDER_KS = 0.001.volts val FEEDER_KV = 0.01.volts.perRotation.perMinute val FEEDER_KA = 0.01.volts.perRotation.perMinute.perSecond - val FEEDER_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute - val FEEDER_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) - val FEEDER_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + val FEEDER_REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_REAL_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_REAL_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) + + val FEEDER_SIM_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute + val FEEDER_SIM_KI: IntegralGain, Volt> = 0.0.volts / (1.0.rotations.perMinute * 1.0.seconds) + val FEEDER_SIM_KD: DerivativeGain, Volt> = 0.0.volts / (1.0.rotations.perMinute / 1.0.seconds) } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index 418840d9..95604908 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -3,83 +3,96 @@ package com.team4099.robot2023.subsystems.feeder import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.FeederConstants -import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.SubsystemBase import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.* import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* +import kotlin.Pair class Feeder(val io: FeederIO): SubsystemBase() { - val inputs = FeederIO.FeederIOInputs() - - var feederTargetVoltage : ElectricalPotential = 0.0.volts - - var lastFeederRunTime = 0.0.seconds - var lastFeederVoltage = 0.0.volts - - var isZeroed = false + val kP = LoggedTunableValue("Feeder/kP", Pair({it.inVoltsPerRotationPerMinute}, {it.volts / 1.0.rotations.perMinute})) + val kI = LoggedTunableValue("Feeder/kI", Pair({it.inVoltsPerRotations}, {it.volts / (1.0.rotations.perMinute * 1.0.seconds)})) + val kD = LoggedTunableValue("Feeder/kD", Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts / 1.0.rotations.perMinute.perSecond})) - private val kP = LoggedTunableValue("Feeder/kP", FlywheelConstants.SHOOTER_FLYWHEEL_KP) - private val kI = LoggedTunableValue("Feeder/kI", FlywheelConstants.SHOOTER_FLYWHEEL_KI) - private val kD = LoggedTunableValue("Feeder/kD", FlywheelConstants.SHOOTER_FLYWHEEL_KD) + val kS = LoggedTunableValue("Feeder/kS", FeederConstants.FEEDER_KS, Pair({it.inVolts}, {it.volts})) + val kV = LoggedTunableValue("Feeder/kV", FeederConstants.FEEDER_KV, Pair({it.inVoltsPerRotationPerMinute}, {it.volts.perRotation.perMinute})) + val kA = LoggedTunableValue("Feeder/kA", FeederConstants.FEEDER_KA, Pair({it.inVoltsPerRotationsPerMinutePerSecond}, {it.volts.perRotation.perMinute.perSecond})) - private val kS = LoggedTunableValue("Feeder/kS", Pair({it.inVoltsPerInch}, {it.volts.perInch})) - private val kV = LoggedTunableValue("Feeder/kV", Pair({it.inVoltsPerInchSeconds}, {it.volts.perInchSeconds})) - private val kA = LoggedTunableValue("Feeder/kA", Pair({it.inVoltsPerInchPerSecond}, {it.volts.perInchPerSecond})) - - var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", FeederConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) - var timeProfileGeneratedAt = Clock.fpgaTime + val inputs = FeederIO.FeederIOInputs() - var currentState = FeederStates.UNINITIALIZED + var feederFeedForward: SimpleMotorFeedforward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + var feederTargetVoltage: ElectricalPotential = 0.0.volts + var feederTargetVelocity: AngularVelocity = FeederConstants.FEED_NOTE_TARGET_VELOCITY + var lastFeederRunTime = 0.0.seconds + var currentState: FeederStates = FeederStates.UNINITIALIZED var currentRequest: Request.FeederRequest = Request.FeederRequest.Idle() - set (value) { - feederTargetVoltage = when (value) { - is Request.FeederRequest.OpenLoop -> { - value.feederVoltage - } - - is Request.FeederRequest.Idle -> { - FeederConstants.FEEDER_INIT_VOLTAGE - } - } - field = value + init { + if (RobotBase.isReal()) { + kP.initDefault(FeederConstants.FEEDER_REAL_KP) + kI.initDefault(FeederConstants.FEEDER_REAL_KI) + kD.initDefault(FeederConstants.FEEDER_REAL_KD) + } else { + kP.initDefault(FeederConstants.FEEDER_SIM_KP) + kI.initDefault(FeederConstants.FEEDER_SIM_KI) + kD.initDefault(FeederConstants.FEEDER_SIM_KD) } + } fun setFeederVoltage(appliedVoltage: ElectricalPotential){ io.setFeederVoltage(appliedVoltage) } + fun setFeederVelocity(velocity: AngularVelocity) { + io.setFeederVelocity(velocity, feederFeedForward.calculate(velocity)) + } + override fun periodic() { io.updateInputs(inputs) - val nextState = when (currentState) { + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + + if (kS.hasChanged() || kV.hasChanged() || kA.hasChanged()) { + feederFeedForward = SimpleMotorFeedforward(kS.get(), kV.get(), kA.get()) + } + + val nextState: FeederStates = when (currentState) { FeederStates.UNINITIALIZED -> { FeederStates.IDLE } FeederStates.IDLE -> { setFeederVoltage(FeederConstants.FEEDER_INIT_VOLTAGE) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } FeederStates.OPEN_LOOP -> { setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime fromRequestToState(currentRequest) } - } - currentState = nextState + FeederStates.TARGETING_VELOCITY -> { + setFeederVelocity(feederTargetVelocity) + lastFeederRunTime = Clock.fpgaTime + fromRequestToState(currentRequest) + } + } } companion object { enum class FeederStates { UNINITIALIZED, IDLE, - OPEN_LOOP; + OPEN_LOOP, + TARGETING_VELOCITY; fun equivalentToRequest(request: Request.FeederRequest): Boolean { return((request is Request.FeederRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.FeederRequest.Idle && this == IDLE)) @@ -90,6 +103,7 @@ class Feeder(val io: FeederIO): SubsystemBase() { return when (request) { is Request.FeederRequest.Idle -> FeederStates.IDLE is Request.FeederRequest.OpenLoop -> FeederStates.OPEN_LOOP + is Request.FeederRequest.TargetingVelocity -> FeederStates.TARGETING_VELOCITY } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 28bdf459..66bfbe0b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -84,7 +84,8 @@ sealed interface Request { } sealed interface FeederRequest : Request { - class OpenLoop(val feederVoltage: ElectricalPotential, val flywheelVoltage: ElectricalPotential): FeederRequest {} class Idle(): FeederRequest {} + class OpenLoop(val feederVoltage: ElectricalPotential): FeederRequest {} + class TargetingVelocity(val feederVelocity: AngularVelocity): FeederRequest {} } }