diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 0244a140..d845f2b9 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -59,4 +59,6 @@ object FlywheelConstants { val FLYWHEEL_KS = 0.001.volts val FLYWHEEL_KV = 0.01.volts/ 1.rotations.perMinute val FLYWHEEL_KA = 0.01.volts/ 1.rotations.perMinute.perSecond + + val FLYWHEEL_INIT_VOLTAGE = 0.0.volts } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt index 18b8f393..b944af45 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -7,6 +7,7 @@ import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.superstructure.Request import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity @@ -53,7 +54,9 @@ class Flywheel (val io: FlywheelIO) { private var hasNote:Boolean = true val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + var currentRequest = Request.FlywheelRequest.OpenLoop(FlywheelConstants.FLYWHEEL_INIT_VOLTAGE) init{ + //TODO figure out what else needs to run in the init function io.configPID(FlywheelConstants.SHOOTER_FLYWHEEL_KP, FlywheelConstants.SHOOTER_FLYWHEEL_KI, FlywheelConstants.SHOOTER_FLYWHEEL_KD) } fun periodic(){ @@ -66,16 +69,21 @@ init{ Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { setFlywheelVoltage(flywheelTargetVoltage) lastFlywheelRunTime = Clock.fpgaTime + + nextState = fromRequestToState(currentRequest) } + //TODO do sensor logic (mayb ask pranav) Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - io.setFlywheelVelocity(inputs.flywheelVelocity, controlEffort)//TODO talk to anshi ab a current velocity var + io.setFlywheelVelocity(inputs.flywheelVelocity, controlEffort) io.setFlywheelVoltage(controlEffort) + lastFlywheelRunTime = Clock.fpgaTime + nextState = fromRequestToState(currentRequest) } } Flywheel.Companion.FlywheelStates.ZERO ->{ - + nextState = fromRequestToState(currentRequest) } } @@ -88,5 +96,13 @@ init{ OPEN_LOOP, TARGETING_VELOCITY, } + inline fun fromRequestToState(request: Request.FlywheelRequest): Flywheel.Companion.FlywheelStates { + return when (request) { + is Request.FlywheelRequest.OpenLoop -> Flywheel.Companion.FlywheelStates.OPEN_LOOP + is Request.FlywheelRequest.TargetingVelocity -> Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY + is Request.FlywheelRequest.Zero -> Flywheel.Companion.FlywheelStates.ZERO + + } + } } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt index 37d6e55a..ee895b91 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -31,6 +31,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{ var flywheelStatorCurrentSignal: StatusSignal var flywheelSupplyCurrentSignal: StatusSignal var flywheelTempSignal: StatusSignal + var flywheelDutyCycle : StatusSignal init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -62,6 +63,7 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{ flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent flywheelTempSignal = flywheelFalcon.deviceTemp + flywheelDutyCycle = flywheelFalcon.dutyCycle MotorChecker.add( "Shooter","Flywheel", @@ -96,6 +98,13 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{ ) } + override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) { + inputs.flywheelVelocity = flywheelSensor.velocity + inputs.flywheelAppliedVoltage = flywheelDutyCycle.value.volts + inputs.flywheelStatorCurrent = flywheelStatorCurrentSignal.value.amps + inputs.flywheelSupplyCurrent = flywheelSupplyCurrentSignal.value.amps + inputs.flywheelTempreature = flywheelTempSignal.value.celsius + } override fun setFlywheelBrakeMode(brake: Boolean) { val motorOutputConfig = MotorOutputConfigs() 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 503f5246..50f3098a 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,12 +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 var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) -/* private val wristflywheelkP = LoggedTunableValue("Wrist/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) private val wristflywheelkI = @@ -33,7 +44,7 @@ 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 @@ -76,9 +87,10 @@ fun periodic(){ var nextState = currentState when (currentState) { ShooterStates.UNINITIALIZED -> { - nextState = ShooterStates.ZERO + nextState = fromRequestToState(currentRequest) } ShooterStates.ZERO ->{ + nextState = fromRequestToState(currentRequest) } ShooterStates.OPEN_LOOP ->{ @@ -94,6 +106,7 @@ fun periodic(){ if (isZeroed == true ){ nextState = fromRequestToState(currentRequest) } + nextState = fromRequestToState(currentRequest) } @@ -101,6 +114,7 @@ fun periodic(){ 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), @@ -115,6 +129,7 @@ fun periodic(){ setWristPosition(wristProfile.calculate(timeElapsed)) //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) + nextState = fromRequestToState(currentRequest) } 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 02093c3c..e700fc35 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -97,7 +97,7 @@ sealed interface Request { } sealed interface FlywheelRequest : Request { class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} - class TargetingVelocity (flywheelVelocity: AngularVelocity) + class TargetingVelocity (flywheelVelocity: AngularVelocity) : FlywheelRequest{} class Zero ():FlywheelRequest{} } }