diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt similarity index 94% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt rename to src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 64bff2c7..74f5903f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -1,5 +1,6 @@ -package com.team4099.robot2023.subsystems.Shooter +package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object FlywheelConstants { 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 da92ac6a..ef11c400 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -2,27 +2,46 @@ package com.team4099.robot2023.subsystems.Shooter import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* +<<<<<<< HEAD import org.team4099.lib.units.* class Flywheel (val io: FlywheelIO) { // TODO: Make shooter constants (kS, kV, kA) var feedforward = SimpleMotorFeedforward() +======= +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perMinute + +class Flywheel (val io: FlywheelIO) { + +>>>>>>> 829a07586e50bf71782342ae9623c560237b502b val inputs = FlywheelIO.FlywheelIOInputs() private val flywheelkS = - LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) - private val flywheelkkV = + LoggedTunableValue("Flywheel/kS", Pair({ it.inVoltsPerRadian }, { it.volts.perRadian })) + private val flywheelkV = LoggedTunableValue( +<<<<<<< HEAD "Flywheel/kV", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) private val flywheelkA = LoggedTunableValue( "Flywheel/kA", Pair({ it.inVoltsPerInchPerSecond}, { it.volts.perInchPerSecond }) +======= + "Flywheel/kV", Pair({ it.inVoltsPerRadianSeconds }, { it.volts.perRadianSeconds }) + ) + private val flywheelkA = + LoggedTunableValue( + "Flywheel/kA", Pair({ it.inVolts.perRadianPerSecond}, { it.volts.perRadianPerSecond }) +>>>>>>> 829a07586e50bf71782342ae9623c560237b502b ) + val flywheelFeedForward = SimpleMotorFeedforward(flywheelkS, flywheelkV, flywheelkA) + var flywheelTargetVoltage: ElectricalPotential = 0.0.volts fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { @@ -32,7 +51,9 @@ class Flywheel (val io: FlywheelIO) { 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 + val desiredVelocity: AngularVelocity = 1800.rotations.perMinute var currentState = Flywheel.Companion.FlywheelStates.UNINITIALIZED + fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -46,9 +67,9 @@ class Flywheel (val io: FlywheelIO) { } Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ - if(hasNote){ + val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - } + io.setFlywheelVoltage(controlEffort) } } Flywheel.Companion.FlywheelStates.ZERO ->{ diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt index 084077f8..e0920b36 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.Shooter import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.controller.SimpleMotorFeedforward import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes @@ -47,6 +48,7 @@ interface FlywheelIO { } } } + fun setFlywheelVoltage (voltage: ElectricalPotential){ } @@ -59,5 +61,10 @@ interface FlywheelIO { fun zeroEncoder(){ } + fun configPID(kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){ + + } } \ 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 f66967d0..73484e57 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -2,16 +2,26 @@ package com.team4099.robot2023.subsystems.Shooter import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.MotorOutputConfigs +import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.FlywheelConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter 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.ctreAngularMechanismSensor +import org.team4099.lib.units.derived.* class FlywheelIOFalcon (private val flywheelFalcon : TalonFX){ -private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelPIDController = TalonFX + private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelSensor = ctreAngularMechanismSensor( flywheelFalcon, @@ -22,6 +32,15 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() val flywheelStatorCurrentSignal: StatusSignal val flywheelSupplyCurrentSignal: StatusSignal val flywheelTempSignal: StatusSignal + private val kP = + LoggedTunableValue("Flywheel/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Flywheel/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val kD = + LoggedTunableValue( + "Flywheel/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts.perInchPerSecond }) init { flywheelFalcon.configurator.apply(TalonFXConfiguration()) @@ -29,11 +48,11 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() flywheelFalcon.configurator.apply(flywheelConfiguration) //TODO fix PID flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KP) + flywheelSensor.proportionalVelocityGainToRawUnits(kP) flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KI) + flywheelSensor.integralVelocityGainToRawUnits(kI) flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstantsConstants.PID.flywheel_KD) + flywheelSensor.derivativeVelocityGainToRawUnits(kD) flywheelConfiguration.Slot0.kV = 0.05425 // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) flywheelConfiguration.CurrentLimits.SupplyCurrentLimit = @@ -65,9 +84,20 @@ private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() ) ) } + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + val PIDConfig = Slot0Configs() + PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP) + PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI) + PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD) + PIDConfig.kV = 0.05425 - + flywheelFalcon.configurator.apply(PIDConfig) + } override fun setFlywheelBrakeMode(brake: Boolean) { 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 6720537e..ede97530 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -18,7 +18,11 @@ import org.team4099.lib.units.perSecond class Shooter (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward +<<<<<<< HEAD private var wristFeedforward: SimpleMotorFeedforward +======= + private var WristFeedforward: SimpleMotorFeedforward(kS,kV,kA) +>>>>>>> 829a07586e50bf71782342ae9623c560237b502b /* private val wristflywheelkP = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt index 5fb6b2d1..757a55f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIO.kt @@ -145,9 +145,9 @@ interface ShooterIO { } fun configWristPID( - kP: ProportionalGain , - kI: IntegralGain , - kD: DerivativeGain , + kP: ProportionalGain , + kI: IntegralGain , + kD: DerivativeGain ){} fun zeroEncoder(){ 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 9100063d..4fd485dc 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt @@ -85,11 +85,10 @@ private val wristSparkMax = CANSparkMax(Constants.Shooter.SHOOTER_WRIST_MOTOR_ID inputs.feederTemperature = feederSparkMax.motorTemperature.celsius*/ } override fun configWristPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain ) { -//TODO fix this please wristPIDController.p = wristSensor.proportionalPositionGainToRawUnits(kP) wristPIDController.i = wristSensor.integralPositionGainToRawUnits(kI) wristPIDController.d = wristSensor.derivativePositionGainToRawUnits(kD) 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 5cabace1..02093c3c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.superstructure import com.team4099.robot2023.config.constants.GamePiece import com.team4099.robot2023.config.constants.NodeTier +import com.team4099.robot2023.subsystems.Shooter.Flywheel import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity @@ -94,4 +95,9 @@ sealed interface Request { class Zero () : ShooterRequest{} } + sealed interface FlywheelRequest : Request { + class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} + class TargetingVelocity (flywheelVelocity: AngularVelocity) + class Zero ():FlywheelRequest{} + } }