diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt new file mode 100644 index 00000000..dd364355 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -0,0 +1,67 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableValue +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* + +class Flywheel (val io: FlywheelIO) { + var feedforward = SimpleMotorFeedforward() + val inputs = FlywheelIO.FlywheelIOInputs() + private val flywheelkS = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val flywheelkkV = + LoggedTunableValue( + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perInchSeconds }) + ) + private val flywheelkA = + LoggedTunableValue( + "Flywheel/kA", Pair({ it.inVoltsPerRadianPerSecond}, { it.volts.perInchPerSecond }) + ) + + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts + fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { + io.setFlywheelVoltage(appliedVoltage) } + + var lastFlywheelRunTime = 0.0.seconds + private var lastFlywheelVoltage = 0.0.volts + private var flywheelInitVoltage = LoggedTunableValue ("Shooter/Initial flywheel Voltage", FlywheelConstants.FLYWHEEEL_INIT_VOLTAGE, Pair({it.inVolts}, {it.volts})) + private var hasNote:Boolean = true + var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ + io.updateInputs(inputs) + var nextState = currentState + when (currentState) { + Flywheel.Companion.FlywheelStates.UNINITIALIZED -> { + nextState = Flywheel.Companion.FlywheelStates.ZERO + } + Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { + setFlywheelVoltage(flywheelTargetVoltage) + lastFlywheelRunTime = Clock.fpgaTime + } + Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ + if (flywheelTargetVoltage != lastFlywheelVoltage){ + if(hasNote){ + + } + } + } + Flywheel.Companion.FlywheelStates.ZERO ->{ + + } + } + + } + + companion object { + enum class FlywheelStates { + UNINITIALIZED, + ZERO, + OPEN_LOOP, + TARGETING_VELOCITY, + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt new file mode 100644 index 00000000..64bff2c7 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt @@ -0,0 +1,44 @@ +package com.team4099.robot2023.subsystems.Shooter + +import org.team4099.lib.units.derived.volts + +object FlywheelConstants { + val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts + val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts + val ROLLER_GEAR_RATIO = 0.0 + + object PID { + + val AUTO_POS_KP: ProportionalGain> + get() { + if (RobotBase.isReal()) { + return 8.0.meters.perSecond / 1.0.meters + } else { + return 7.0.meters.perSecond / 1.0.meters + } + } + val AUTO_POS_KI: IntegralGain> + get() { + if (RobotBase.isReal()) { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } else { + return 0.0.meters.perSecond / (1.0.meters * 1.0.seconds) + } + } + + val AUTO_POS_KD: DerivativeGain> + get() { + if (RobotBase.isReal()) { + return (0.05.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } else { + return (0.0.meters.perSecond / (1.0.meters.perSecond)).metersPerSecondPerMetersPerSecond + } + + } + } + val FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds + val FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps + +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt new file mode 100644 index 00000000..084077f8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -0,0 +1,63 @@ +package com.team4099.robot2023.subsystems.Shooter + +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.* +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface FlywheelIO { + + class FlywheelIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + var rollerAppliedVoltage = 0.volts + var rollerStatorCurrent = 0.amps + var rollerSupplyCurrent = 0.amps + var rollerTempreature = 0.celsius + + override fun toLog(table: LogTable) { + table.put("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond) + table.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + table.put("rollerStatorCurrent", rollerStatorCurrent.inAmperes) + table.put("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes) + table.put("rollerTempreature", rollerTempreature.inCelsius) + } + override fun fromLog(table: LogTable) { + //roller logs + table.get("rollerVelocityRPM", rollerVelocity.inRadiansPerSecond).let { + rollerVelocity = it.radians.perSecond + } + table.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts).let { + rollerAppliedVoltage = it.volts + } + table.get("rollerStatorCurrent", rollerStatorCurrent.inAmperes).let { + rollerStatorCurrent = it.amps + } + table.get("rollerSupplyCurrent", rollerSupplyCurrent.inAmperes).let { + rollerSupplyCurrent = it.amps + + } + table.get("rollerTempreature", rollerTempreature.inCelsius).let { + rollerTempreature = it.celsius + } + } + } + fun setFlywheelVoltage (voltage: ElectricalPotential){ + + } + fun setFlywheelBrakeMode (brake: Boolean){ + + } + fun updateInputs(inputs:FlywheelIOInputs){ + + } + fun zeroEncoder(){ + + } + +} \ 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 new file mode 100644 index 00000000..f66967d0 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -0,0 +1,87 @@ +package com.team4099.robot2023.subsystems.Shooter + +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.hardware.TalonFX +import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.ctreAngularMechanismSensor + +class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ +private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelSensor = + ctreAngularMechanismSensor( + flywheelFalcon, + FlywheelConstants.ROLLER_GEAR_RATIO, + FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, + + ) + val flywheelStatorCurrentSignal: StatusSignal + val flywheelSupplyCurrentSignal: StatusSignal + val flywheelTempSignal: StatusSignal + init { + flywheelFalcon.configurator.apply(TalonFXConfiguration()) + + flywheelFalcon.clearStickyFaults() + flywheelFalcon.configurator.apply(flywheelConfiguration) +//TODO fix PID + flywheelConfiguration.Slot0.kP = + flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelConfiguration.Slot0.kI = + flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelConfiguration.Slot0.kD = + flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelConfiguration.Slot0.kV = 0.05425 + // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) + flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + flywheelFalcon.configurator.apply(flywheelConfiguration) + + flywheelStatorCurrentSignal = flywheelFalcon.statorCurrent + flywheelSupplyCurrentSignal = flywheelFalcon.supplyCurrent + flywheelTempSignal = flywheelFalcon.deviceTemp + + MotorChecker.add( + "flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + } + + + + + + override fun setFlywheelBrakeMode(brake: Boolean) { + val motorOutputConfig = MotorOutputConfigs() + + if (brake) { + motorOutputConfig.NeutralMode = NeutralModeValue.Brake + } else { + motorOutputConfig.NeutralMode = NeutralModeValue.Coast + } + flywheelFalcon.configurator.apply(motorOutputConfig) + } + override fun zeroEncoder(){ + //TODO finish zero encoder fun (ask sumone what the encoder for falcon is) + flywheelFalcon + } +} \ No newline at end of file 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 f9b1c3d7..8064d58f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -32,7 +32,7 @@ class Shooter (val io: ShooterIO){ "wrist/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) )*/ var currentState = ShooterStates.UNINITIALIZED - var flywheelTargetVoltage : ElectricalPotential= 0.0.volts + //var flywheelTargetVoltage : ElectricalPotential= 0.0.volts var wristTargetVoltage : ElectricalPotential = 0.0.volts var feederTargetVoltage : ElectricalPotential = 0.0.volts /* fun setflywheelVoltage(appliedVoltage: ElectricalPotential){ @@ -56,7 +56,6 @@ class Shooter (val io: ShooterIO){ private var feederInitVoltage = LoggedTunableValue ("Shooter/Initial Feeder Voltage", ShooterConstants.FEEDER_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) */private var wristInitVoltage = LoggedTunableValue ("Shooter/Initial Wrist Voltage", ShooterConstants.WRIST_INIT_VOLTAGE, Pair({it.inVolts},{it.volts})) private var timeProfileGeneratedAt = 0.0.seconds - //TODO ask aanshi wtf to pass in for init parameters var currentRequest = Request.ShooterRequest.OpenLoop( ShooterConstants.WRIST_INIT_VOLTAGE, //ShooterConstants.ROLLLER_INIT_VOLTAGE, @@ -77,7 +76,6 @@ fun periodic(){ nextState = ShooterStates.ZERO } ShooterStates.ZERO ->{ -//TODO create zero encoder if we get one } ShooterStates.OPEN_LOOP ->{ @@ -112,7 +110,7 @@ fun periodic(){ } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt setWristPosition(wristProfile.calculate(timeElapsed)) - //TODO implement set wrist pos function + //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt index 060434f7..9100063d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -13,7 +13,6 @@ import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue -//TODO write a kraken file object ShooterIONeo : ShooterIO{ //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax,