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 31afc6c6..75ef3d16 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -132,6 +132,10 @@ object Constants { const val REV_ENCODER_PORT = 7 } + object Feeder { + + } + object Led { const val LED_CANDLE_ID = 61 const val LED_BLINKEN_ID = 1 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 9bd6e98d..0113d826 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FeederConstants.kt @@ -1,21 +1,17 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.derived.rotations -import org.team4099.lib.units.derived.perRotation -import org.team4099.lib.units.derived.ProportionalGain -import org.team4099.lib.units.derived.IntegralGain -import org.team4099.lib.units.derived.DerivativeGain -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.* import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object FeederConstants { val FEEDER_INIT_VOLTAGE = 0.0.volts + val VOLTAGE_COMPENSATION = 0.0.volts + + const val FEEDER_GEAR_RATIO = 0 + const val FEEDER_INERTIA = 0 // TODO: Add value to Feeder target velocity val FEED_NOTE_TARGET_VELOCITY = 0.rotations.perMinute diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index 3bd688df..eec37766 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inSeconds @@ -18,9 +19,22 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward - private var WristFeedforward: SimpleMotorFeedforward() + private val wristkS = + LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) + ) + private val wristlkV = + LoggedTunableValue( + "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + ) + private val wristkA = + LoggedTunableValue( + "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }) + ) + val flywheelFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) + + + -/* private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) private val wristflywheelkI = @@ -30,14 +44,14 @@ class Shooter (val io: ShooterIO){ private val wristflywheelkD = LoggedTunableValue( "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) - )*/ + ) var currentState = ShooterStates.UNINITIALIZED //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts - /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ - io.setflywheelVoltage(appliedVoltage) - }*/ + /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ + io.setflywheelVoltage(appliedVoltage) + }*/ fun setWristVoltage(appliedVoltage: ElectricalPotential){ io.setWristVoltage(appliedVoltage) } @@ -60,7 +74,7 @@ class Shooter (val io: ShooterIO){ ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, //ShooterConstants.FEEDER_INIT_VOLTAGE - ) + ) private var wristProfile = TrapezoidProfile( wristConstraints, @@ -68,51 +82,55 @@ class Shooter (val io: ShooterIO){ TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) -fun periodic(){ - io.updateInputs(inputs) - var nextState = currentState - when (currentState) { - ShooterStates.UNINITIALIZED -> { - nextState = ShooterStates.ZERO - } - ShooterStates.ZERO ->{ - } - - ShooterStates.OPEN_LOOP ->{ - /* - setflywheelVoltage(flywheelTargetVoltage) - lastflywheelRunTime = Clock.fpgaTime -*/ - setWristVoltage(wristTargetVoltage) - lastWristRunTime = Clock.fpgaTime - - /* setFeederVoltage(feederTargetVoltage) - lastFeederRunTime = Clock.fpgaTime*/ - if (isZeroed == true ){ + fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + ShooterStates.UNINITIALIZED -> { + nextState = fromRequestToState(currentRequest) + } + ShooterStates.ZERO ->{ nextState = fromRequestToState(currentRequest) } - } + ShooterStates.OPEN_LOOP ->{ + /* + setflywheelVoltage(flywheelTargetVoltage) + lastflywheelRunTime = Clock.fpgaTime + */ + setWristVoltage(wristTargetVoltage) + lastWristRunTime = Clock.fpgaTime + + /* setFeederVoltage(feederTargetVoltage) + lastFeederRunTime = Clock.fpgaTime*/ + if (isZeroed == true ){ + nextState = fromRequestToState(currentRequest) + } + nextState = fromRequestToState(currentRequest) - ShooterStates.TARGETING_POSITION ->{ - - if (wristPositionTarget!=lastWristPositionTarget){ - val preProfileGenerate = Clock.fpgaTime - wristProfile = TrapezoidProfile( - wristConstraints, - TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), - TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) - ) - val postProfileGenerate = Clock.fpgaTime - Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) - timeProfileGeneratedAt = Clock.fpgaTime - lastWristPositionTarget = wristPositionTarget } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition( WristFeedforward, wristProfile.calculate(timeElapsed)) - //TODO fix this error - Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) - } + + ShooterStates.TARGETING_POSITION ->{ + + if (wristPositionTarget!=lastWristPositionTarget){ + val preProfileGenerate = Clock.fpgaTime + //TODO figure out how to implment feedforward here. + wristProfile = TrapezoidProfile( + wristConstraints, + TrapezoidProfile.State(wristPositionTarget, 0.0.radians.perSecond), + TrapezoidProfile.State(inputs.wristPostion, inputs.wristVelocity) + ) + val postProfileGenerate = Clock.fpgaTime + Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) + timeProfileGeneratedAt = Clock.fpgaTime + lastWristPositionTarget = wristPositionTarget + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + setWristPosition(wristProfile.calculate(timeElapsed)) + //TODO fix this error + Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) + } @@ -137,6 +155,4 @@ fun periodic(){ } } -} - - +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 146feeb2..8de21e7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.feeder +import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs import org.team4099.lib.units.* diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt new file mode 100644 index 00000000..1fa3064e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIOSim.kt @@ -0,0 +1,53 @@ +package com.team4099.robot2023.subsystems.feeder + +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.FeederConstants +import edu.wpi.first.math.system.plant.DCMotor +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 + +class FeederIOSim : FeederIO { + private val feederSim: FlywheelSim = FlywheelSim( + DCMotor.getNEO(1), + FeederConstants.FEEDER_GEAR_RATIO, + FeederConstants.FEEDER_INERTIA + ) + + override fun updateInputs(inputs: FeederIO.FeederIOInputs) { + feederSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.feederVelocity = feederSim.getAngularVelocityRPM().rotations.perMinute + inputs.feederAppliedVoltage = appliedVoltage + inputs.feederSupplyCurrent = 0.amps + inputs.feederStatorCurrent = feederSim.currentDrawAmps.amps + inputs.feederTemp = 0.0.celsius + } + + override fun setRollerVoltage(voltage: ElectricalPotential) { + appliedVoltage = voltage + feederSim.setInputVoltage( + clamp( + voltage, + -FeederConstants.VOLTAGE_COMPENSATION, + FeederConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + } + + override fun setFeederVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { + + } + + override fun configPID(kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt>) { + + } +}