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 d845f2b9..141678ee 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -12,42 +12,20 @@ import org.team4099.lib.units.perSecond object FlywheelConstants { val FLYWHEEEL_INIT_VOLTAGE = 0.0.volts - val FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts + val RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION = 12.volts + val LEFT_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 RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps + val RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val RIGHT_flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds + val RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps - 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 + val LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 0.0.seconds + val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 0.0.amps val SHOOTER_FLYWHEEL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute 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 b944af45..309b473c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Flywheel.kt @@ -72,7 +72,7 @@ init{ nextState = fromRequestToState(currentRequest) } - //TODO do sensor logic (mayb ask pranav) + //TODO do sensor logic (mayb ask p) Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY ->{ if (flywheelTargetVoltage != lastFlywheelVoltage){ val controlEffort: ElectricalPotential = flywheelFeedForward.calculate(desiredVelocity) 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 ee895b91..cafa44ff 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/FlywheelIOFalcon.kt @@ -18,92 +18,174 @@ import org.team4099.lib.units.base.* import org.team4099.lib.units.ctreAngularMechanismSensor import org.team4099.lib.units.derived.* -class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{ +class FlywheelIOFalcon (private val flywheelRightFalcon: TalonFX, private val flywheelLeftFalcon: TalonFX) : FlywheelIO{ - private val flywheelConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val flywheelSensor = + private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + private val flywheelRightSensor = ctreAngularMechanismSensor( - flywheelFalcon, + flywheelRightFalcon, FlywheelConstants.ROLLER_GEAR_RATIO, - FlywheelConstants.FLYWHEEL_VOLTAGE_COMPENSATION, + FlywheelConstants.RIGHT_FLYWHEEL_VOLTAGE_COMPENSATION, ) - var flywheelStatorCurrentSignal: StatusSignal - var flywheelSupplyCurrentSignal: StatusSignal - var flywheelTempSignal: StatusSignal - var flywheelDutyCycle : StatusSignal + + private val flywheelLeftSensor = + ctreAngularMechanismSensor( + flywheelLeftFalcon, + FlywheelConstants.ROLLER_GEAR_RATIO, + FlywheelConstants.LEFT_FLYWHEEL_VOLTAGE_COMPENSATION, + + ) + + var rightFlywheelStatorCurrentSignal: StatusSignal + var rightFlywheelSupplyCurrentSignal: StatusSignal + var rightFlywheelTempSignal: StatusSignal + var rightFlywheelDutyCycle : StatusSignal + + var leftFlywheelStatorCurrentSignal: StatusSignal + var leftFlywheelSupplyCurrentSignal: StatusSignal + var leftFlywheelTempSignal: StatusSignal + var leftFlywheelDutyCycle : StatusSignal init { - flywheelFalcon.configurator.apply(TalonFXConfiguration()) - - flywheelFalcon.clearStickyFaults() - flywheelFalcon.configurator.apply(flywheelConfiguration) -//TODO fix PID - flywheelConfiguration.Slot0.kP = - flywheelSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) - flywheelConfiguration.Slot0.kI = - flywheelSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) - flywheelConfiguration.Slot0.kD = - flywheelSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) - flywheelConfiguration.Slot0.kV = 0.05425 + flywheelRightFalcon.configurator.apply(TalonFXConfiguration()) + flywheelLeftFalcon.configurator.apply(TalonFXConfiguration()) + + flywheelRightFalcon.clearStickyFaults() + flywheelRightFalcon.configurator.apply(flywheelRightConfiguration) + + flywheelLeftFalcon.clearStickyFaults() + flywheelLeftFalcon.configurator.apply(flywheelLeftConfiguration) + + flywheelRightConfiguration.Slot0.kP = + flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) + flywheelRightConfiguration.Slot0.kI = + flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) + flywheelRightConfiguration.Slot0.kD = + flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) + flywheelRightConfiguration.Slot0.kV = 0.05425 + + flywheelLeftConfiguration.Slot0.kP = + flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KP) + flywheelRightConfiguration.Slot0.kI = + flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KI) + flywheelRightConfiguration.Slot0.kD = + flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.SHOOTER_FLYWHEEL_KD) + flywheelRightConfiguration.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 - flywheelDutyCycle = flywheelFalcon.dutyCycle + + flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.RIGHT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.RIGHT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelRightConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelRightConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.RIGHT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelRightConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_SUPPLY_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentThreshold = + FlywheelConstants.LEFT_FLYWHEEL_THRESHOLD_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.SupplyTimeThreshold = + FlywheelConstants.LEFT_flywheel_TRIGGER_THRESHOLD_TIME.inSeconds + flywheelLeftConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimit = + FlywheelConstants.LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT.inAmperes + flywheelLeftConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + + flywheelRightConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + flywheelLeftConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + + flywheelLeftFalcon.configurator.apply(flywheelRightConfiguration) + flywheelRightFalcon.configurator.apply(flywheelLeftConfiguration) + + rightFlywheelStatorCurrentSignal = flywheelRightFalcon.statorCurrent + rightFlywheelSupplyCurrentSignal = flywheelRightFalcon.supplyCurrent + rightFlywheelTempSignal = flywheelRightFalcon.deviceTemp + rightFlywheelDutyCycle = flywheelRightFalcon.dutyCycle + + leftFlywheelStatorCurrentSignal = flywheelLeftFalcon.statorCurrent + leftFlywheelSupplyCurrentSignal = flywheelLeftFalcon.supplyCurrent + leftFlywheelTempSignal = flywheelLeftFalcon.deviceTemp + leftFlywheelDutyCycle = flywheelLeftFalcon.dutyCycle MotorChecker.add( "Shooter","Flywheel", MotorCollection( - mutableListOf(Falcon500(flywheelFalcon, "Flywheel Motor")), - FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT, + mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, 90.celsius, - FlywheelConstants.FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + + 110.celsius + ) + ) + MotorChecker.add( + "Shooter","Flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + + 110.celsius + ) + ) + MotorChecker.add( + "Shooter","Flywheel", + MotorCollection( + mutableListOf(Falcon500(flywheelRightFalcon, "Flywheel Right Motor")), + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT, + 90.celsius, + FlywheelConstants.RIGHT_FLYWHEEL_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius ) ) } + //TODO do the checks for pid and feedforward change override fun configPID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> + rightkP: ProportionalGain, Volt>, + rightkI: IntegralGain, Volt>, + rightkD: DerivativeGain, Volt>, + leftkP: ProportionalGain, Volt>, + leftkI: IntegralGain, Volt>, + leftkD: DerivativeGain, Volt> ) { val PIDConfig = Slot0Configs() - PIDConfig.kP = flywheelSensor.proportionalVelocityGainToRawUnits(kP) - PIDConfig.kI = flywheelSensor.integralVelocityGainToRawUnits(kI) - PIDConfig.kD = flywheelSensor.derivativeVelocityGainToRawUnits(kD) + PIDConfig.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(rightkP) + PIDConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(rightkI) + PIDConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(rightkD) PIDConfig.kV = 0.05425 - flywheelFalcon.configurator.apply(PIDConfig) + PIDConfig.kP = flywheelLeftSensor.proportionalVelocityGainToRawUnits(leftkP) + PIDConfig.kI = flywheelLeftSensor.integralVelocityGainToRawUnits(leftkI) + PIDConfig.kD = flywheelLeftSensor.derivativeVelocityGainToRawUnits(leftkD) + PIDConfig.kV = 0.05425 + + flywheelRightFalcon.configurator.apply(PIDConfig) + flywheelLeftFalcon.configurator.apply(PIDConfig) } override fun setFlywheelVelocity(angularVelocity: AngularVelocity, feedforward: ElectricalPotential){ - flywheelFalcon.setControl(0, - flywheelSensor.velocityToRawUnits(angularVelocity), + flywheelRightFalcon.setControl(0, + flywheelRightSensor.velocityToRawUnits(angularVelocity), DemandType.ArbitraryFeedForward, feedforward.inVolts ) } - +//TODO create IO for the left falcon 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 + inputs.flywheelVelocity = flywheelRightSensor.velocity + inputs.flywheelAppliedVoltage = rightFlywheelDutyCycle.value.volts + inputs.flywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps + inputs.flywheelSupplyCurrent = rightFlywheelSupplyCurrentSignal.value.amps + inputs.flywheelTempreature = rightFlywheelTempSignal.value.celsius } override fun setFlywheelBrakeMode(brake: Boolean) { @@ -114,10 +196,10 @@ class FlywheelIOFalcon (private val flywheelFalcon : TalonFX) : FlywheelIO{ } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } - flywheelFalcon.configurator.apply(motorOutputConfig) + flywheelRightFalcon.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/Wrist.kt similarity index 89% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt index 50f3098a..698e3a57 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Wrist.kt @@ -3,20 +3,18 @@ package com.team4099.robot2023.subsystems.Shooter import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.ShooterConstants -import com.team4099.robot2023.subsystems.Shooter.ShooterIONeo.setWristPosition +import com.team4099.robot2023.subsystems.Shooter.WristIONeo.setWristPosition 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 import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.* import org.team4099.lib.units.perSecond -class Shooter (val io: ShooterIO){ +class Wrist (val io: ShooterIO){ val inputs = ShooterIO.ShooterIOInputs() //TODO do feedforward private val wristkS = @@ -24,15 +22,15 @@ class Shooter (val io: ShooterIO){ ) private val wristlkV = LoggedTunableValue( - "Wrist/kV", Pair({ it.inVoltsPerRotaionPerMinute }, { it.volts.perRotationPerMinute }) + "Wrist/kV", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) ) private val wristkA = LoggedTunableValue( - "Wrist/kA", Pair({ it.inVoltsPerRotationPerMinutePerSecond}, { it.volts.perRotationPerMinutePerSecond }) + "Wrist/kA", Pair({ it.inVoltsPerDegreePerSecond.perSecond}, { it.volts.perDegreePerSecond.perSecond }) ) - val flywheelFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) - + val wristFeedForward = SimpleMotorFeedforward(wristkS.get(), wristlkV.get(), wristkA.get()) + override fun () private val wristflywheelkP = @@ -81,7 +79,9 @@ class Shooter (val io: ShooterIO){ TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond), TrapezoidProfile.State(-1337.radians, -1337.radians.perSecond) ) + fun setWristPosition(setPoint : TrapezoidProfile.State){ + } fun periodic(){ io.updateInputs(inputs) var nextState = currentState @@ -124,9 +124,11 @@ fun periodic(){ Logger.recordOutput("/Shooter/ProfileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) timeProfileGeneratedAt = Clock.fpgaTime lastWristPositionTarget = wristPositionTarget + } val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - setWristPosition(wristProfile.calculate(timeElapsed)) + val setPoint: TrapezoidProfile.State= wristProfile.calculate(timeElapsed) + setWristPosition(setPoint) //TODO fix this error Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed)) nextState = fromRequestToState(currentRequest) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt similarity index 98% rename from src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt index 4fd485dc..6ac7cff9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/ShooterIONeo.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/WristIONeo.kt @@ -6,14 +6,13 @@ import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ShooterConstants -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.derived.* import org.team4099.lib.units.sparkMaxAngularMechanismSensor import kotlin.math.absoluteValue -object ShooterIONeo : ShooterIO{ +object WristIONeo : ShooterIO{ //private val rollerSparkMax = CANSparkMax(Constants.Shooter.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) //private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, //ShooterConstants.ROLLER_GEAR_RATIO,