diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt new file mode 100644 index 00000000..8e3a9653 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -0,0 +1,64 @@ +package com.team4099.lib.phoenix6 + +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.inRotationsPerSecondPerSecond +import org.team4099.lib.units.perSecond + +class VelocityVoltage(var velocity: AngularVelocity, + var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + var enableFOC:Boolean = true, + var feedforward: ElectricalPotential = 0.0.volts, + var slot:Int = 0, + var overrideBrakeDurNeutral: Boolean = false, + var limitForwardMotion: Boolean = false, + var limitReverseMotion: Boolean = false){ + + val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } + + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} \ 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 1e5451b0..396620a6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -35,7 +35,7 @@ class Flywheel (val io: FlywheelIO) { ) private val flywheelkV = LoggedTunableValue( - "Flywheel/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + "Flywheel/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotation.perMinute }) ) private val flywheelkA = LoggedTunableValue( @@ -57,14 +57,17 @@ class Flywheel (val io: FlywheelIO) { 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(){ io.updateInputs(inputs) + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get()) + } var nextState = currentState when (currentState) { Flywheel.Companion.FlywheelStates.UNINITIALIZED -> { - nextState = Flywheel.Companion.FlywheelStates.ZERO + nextState = Flywheel.Companion.FlywheelStates.OPEN_LOOP } Flywheel.Companion.FlywheelStates.OPEN_LOOP -> { setFlywheelVoltage(flywheelTargetVoltage) @@ -72,20 +75,17 @@ init{ nextState = fromRequestToState(currentRequest) } - //TODO do sensor logic (mayb ask pranav) - //TODO add in the left flywheel to this function mayb ask ab dif ff for motors + Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) - io.setFlywheelVelocity(inputs.rightFlywheelVelocity, controlEffort) + io.setFlywheelVelocity(inputs.rightFlywheelVelocity, inputs.leftFlywheelVelocity, controlEffort) io.setFlywheelVoltage(controlEffort) lastFlywheelRunTime = Clock.fpgaTime nextState = fromRequestToState(currentRequest) } } - Flywheel.Companion.FlywheelStates.ZERO ->{ - nextState = fromRequestToState(currentRequest) - } + } } @@ -93,7 +93,6 @@ init{ companion object { enum class FlywheelStates { UNINITIALIZED, - ZERO, OPEN_LOOP, TARGETING_VELOCITY, } @@ -101,8 +100,6 @@ init{ 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 - } } } 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 556f35b8..883a6784 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIO.kt @@ -77,7 +77,7 @@ interface FlywheelIO { leftFlywheelTempreature = it.celsius } } - + } fun setFlywheelVoltage(voltage: ElectricalPotential) { } @@ -94,17 +94,11 @@ interface FlywheelIO { } - fun zeroEncoder() { - - } - fun configPID( kP: ProportionalGain, Volt>, kI: IntegralGain, Volt>, kD: DerivativeGain, Volt> - ) { + ) {} - } - } } \ 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/FlywheelIOTalon.kt similarity index 77% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOTalon.kt index 71a7d4e7..2dafc2fd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOTalon.kt @@ -4,28 +4,28 @@ 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.team4099.lib.phoenix6.VelocityVoltage 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.controller.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.* import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* +import org.team4099.lib.units.inRotationsPerSecond -class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val flywheelLeftFalcon: TalonFX) : FlywheelIO{ +class FlywheelIOTalon (private val flywheelRightTalon: TalonFX, private val flywheelLeftTalon: TalonFX) : FlywheelIO{ private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val flywheelRightSensor = ctreAngularMechanismSensor( - flywheelRightFalcon, + flywheelRightTalon, FlywheelConstants.ROLLER_GEAR_RATIO, FlywheelConstants.RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION, @@ -33,7 +33,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl private val flywheelLeftSensor = ctreAngularMechanismSensor( - flywheelLeftFalcon, + flywheelLeftTalon, FlywheelConstants.ROLLER_GEAR_RATIO, FlywheelConstants.LEFT_FLYWHEEL_VOLTAGE_COMPENSATION, @@ -49,14 +49,14 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl var leftFlywheelTempSignal: StatusSignal var leftFlywheelDutyCycle : StatusSignal init { - flywheelRightFalcon.configurator.apply(TalonFXConfiguration()) - flywheelLeftFalcon.configurator.apply(TalonFXConfiguration()) + flywheelRightTalon.configurator.apply(TalonFXConfiguration()) + flywheelLeftTalon.configurator.apply(TalonFXConfiguration()) - flywheelRightFalcon.clearStickyFaults() - flywheelRightFalcon.configurator.apply(flywheelRightConfiguration) + flywheelRightTalon.clearStickyFaults() + flywheelRightTalon.configurator.apply(flywheelRightConfiguration) - flywheelLeftFalcon.clearStickyFaults() - flywheelLeftFalcon.configurator.apply(flywheelLeftConfiguration) + flywheelLeftTalon.clearStickyFaults() + flywheelLeftTalon.configurator.apply(flywheelLeftConfiguration) flywheelRightConfiguration.Slot0.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) @@ -101,23 +101,23 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - flywheelLeftFalcon.configurator.apply(flywheelRightConfiguration) - flywheelRightFalcon.configurator.apply(flywheelLeftConfiguration) + flywheelLeftTalon.configurator.apply(flywheelRightConfiguration) + flywheelRightTalon.configurator.apply(flywheelLeftConfiguration) - rightFlywheelStatorCurrentSignal = flywheelRightFalcon.statorCurrent - rightFlywheelSupplyCurrentSignal = flywheelRightFalcon.supplyCurrent - rightFlywheelTempSignal = flywheelRightFalcon.deviceTemp - rightFlywheelDutyCycle = flywheelRightFalcon.dutyCycle + rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent + rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent + rightFlywheelTempSignal = flywheelRightTalon.deviceTemp + rightFlywheelDutyCycle = flywheelRightTalon.dutyCycle - leftFlywheelStatorCurrentSignal = flywheelLeftFalcon.statorCurrent - leftFlywheelSupplyCurrentSignal = flywheelLeftFalcon.supplyCurrent - leftFlywheelTempSignal = flywheelLeftFalcon.deviceTemp - leftFlywheelDutyCycle = flywheelLeftFalcon.dutyCycle + leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent + leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent + leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp + leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle MotorChecker.add( "Shooter","Flywheel", MotorCollection( - mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, 90.celsius, FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, @@ -128,7 +128,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl MotorChecker.add( "Shooter","Flywheel", MotorCollection( - mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, 90.celsius, FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, @@ -139,7 +139,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl MotorChecker.add( "Shooter","Flywheel", MotorCollection( - mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + mutableListOf(Falcon500(flywheelRightTalon, "Flywheel Right Motor")), FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, 90.celsius, FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, @@ -169,21 +169,14 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl PIDConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(leftkD) PIDConfig.kV = 0.05425 - flywheelRightFalcon.configurator.apply(PIDConfig) - flywheelLeftFalcon.configurator.apply(PIDConfig) + flywheelRightTalon.configurator.apply(PIDConfig) + flywheelLeftTalon.configurator.apply(PIDConfig) } //TODO add left flywheel to this override fun setFlywheelVelocity(rightAngularVelocity: AngularVelocity, leftAngularVelocity: AngularVelocity, feedforward: ElectricalPotential){ - flywheelRightFalcon.setControl(0, - flywheelRightSensor.velocityToRawUnits(rightAngularVelocity), - DemandType.ArbitraryFeedForward, - feedforward.inVolts - ) - flywheelLeftFalcon.setControl(0, - flywheelRightSensor.velocityToRawUnits(leftAngularVelocity), - DemandType.ArbitraryFeedForward, - feedforward.inVolts - ) + flywheelRightTalon.setControl(VelocityVoltage(rightAngularVelocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6) + + flywheelLeftTalon.setControl(VelocityVoltage(leftAngularVelocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6) } override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) { inputs.rightFlywheelVelocity = flywheelRightSensor.velocity @@ -207,11 +200,7 @@ class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val fl } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } - flywheelRightFalcon.configurator.apply(motorOutputConfig) - flywheelRightFalcon.configurator.apply(motorOutputConfig) - } - override fun zeroEncoder(){ - //TODO finish zero encoder fun (ask sumone what the encoder for falcon is) - + flywheelRightTalon.configurator.apply(motorOutputConfig) + flywheelRightTalon.configurator.apply(motorOutputConfig) } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt index e0d89a03..41a5c02c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt @@ -19,12 +19,23 @@ import org.team4099.lib.units.perSecond class Wrist (val io: WristIO) { val inputs = WristIO.ShooterIOInputs() - var wristFeedForward: ArmFeedforward = - ArmFeedforward( - WristConstants.WRIST_KS, - WristConstants.WRRIST_KG, - WristConstants.WRIST_KV, - WristConstants.WRIST_KA + private val wristkS = + LoggedTunableValue("Wrist/kS", Pair({ it.inVolts }, { it.volts}) + ) + private val wristkV = + LoggedTunableValue( + "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + ) + private val wristkA = + LoggedTunableValue( + "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond })) + private val wristkG = LoggedTunableValue("Wrist/kG", Pair({ it.inVolts }, { it.volts} )) + + var wristFeedForward: ArmFeedforward = ArmFeedforward( + wristkS.get(), + wristkG.get(), + wristkV.get(), + wristkA.get() ) @@ -125,6 +136,14 @@ class Wrist (val io: WristIO) { if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { io.configWristPID(wristkP.get(), wristkI.get(), wristkD.get()) } + if(wristkA.hasChanged()||wristkV.hasChanged()||wristkG.hasChanged()||wristkS.hasChanged()){ + wristFeedForward = ArmFeedforward( + wristkS.get(), + wristkG.get(), + wristkV.get(), + wristkA.get() + ) + } Logger.processInputs("Wrist", inputs) Logger.recordOutput("Wrist/currentState", currentState.name) 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 c2153249..472fd670 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -92,6 +92,5 @@ sealed interface Request { sealed interface FlywheelRequest : Request { class OpenLoop (flywheelVoltage: ElectricalPotential):FlywheelRequest{} class TargetingVelocity (flywheelVelocity: AngularVelocity) : FlywheelRequest{} - class Zero ():FlywheelRequest{} } }