From 2ac4df3605d67ef995dea36f9ccec757047a1ade Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Sat, 27 Jan 2024 19:25:42 -0500 Subject: [PATCH] Remove leftflywheel motor and fix setPositon and Voltage functions to not create tons of objects also added torque and duty cycle to all talon files. --- .../subsystems/flywheel/FlywheelIO.kt | 2 + .../subsystems/flywheel/FlywheelIOTalon.kt | 80 ++++++------------- .../robot2023/subsystems/wrist/Wrist.kt | 1 + .../robot2023/subsystems/wrist/WristIO.kt | 14 ++-- .../subsystems/wrist/WristIOTalon.kt | 18 ++++- 5 files changed, 48 insertions(+), 67 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt index 8f7ca6ac..66938ed0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt @@ -29,6 +29,8 @@ interface FlywheelIO { var rightFlywheelStatorCurrent = 0.amps var rightFlywheelSupplyCurrent = 0.amps var rightFlywheelTemperature = 0.celsius + var rightFlywheelDutyCycle = 0.0.volts + var rightFlywheelTorque = 0.0 var leftFlywheelVelocity = 0.0.rotations.perMinute var leftFlywheelAppliedVoltage = 0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt index 4b81145f..a02cc17f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIOTalon.kt @@ -9,12 +9,12 @@ import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.lib.phoenix6.VelocityVoltage import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_LEFT_MOTOR_ID import com.team4099.robot2023.config.constants.Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID 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 com.team4099.robot2023.subsystems.wrist.WristIOTalon import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps @@ -28,24 +28,18 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt +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.perSecond object FlywheelIOTalon : FlywheelIO { - private val flywheelLeftTalon: TalonFX = TalonFX(FLYWHEEL_LEFT_MOTOR_ID) private val flywheelRightTalon: TalonFX = TalonFX(FLYWHEEL_RIGHT_MOTOR_ID) - private val flywheelLeftConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val flywheelRightConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val flywheelLeftSensor = - ctreAngularMechanismSensor( - flywheelLeftTalon, - FlywheelConstants.LEFT_GEAR_RATIO, - FlywheelConstants.VOLTAGE_COMPENSATION, - ) + var velocityRequest = VelocityVoltage(-1337.degrees.perSecond, slot = 0, feedforward = -1337.volts) private val flywheelRightSensor = ctreAngularMechanismSensor( @@ -54,36 +48,22 @@ object FlywheelIOTalon : FlywheelIO { FlywheelConstants.VOLTAGE_COMPENSATION, ) - var leftFlywheelStatorCurrentSignal: StatusSignal - var leftFlywheelSupplyCurrentSignal: StatusSignal - var leftFlywheelTempSignal: StatusSignal - var leftFlywheelDutyCycle: StatusSignal var rightFlywheelStatorCurrentSignal: StatusSignal var rightFlywheelSupplyCurrentSignal: StatusSignal var rightFlywheelTempSignal: StatusSignal var rightFlywheelDutyCycle: StatusSignal + var motorVoltage : StatusSignal + var motorTorque :StatusSignal init { - flywheelLeftTalon.configurator.apply(TalonFXConfiguration()) - flywheelRightTalon.configurator.apply(TalonFXConfiguration()) - - flywheelLeftTalon.clearStickyFaults() - flywheelRightTalon.clearStickyFaults() - - flywheelLeftConfiguration.Slot0.kP = - flywheelLeftSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP) - flywheelLeftConfiguration.Slot0.kI = - flywheelLeftSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI) - flywheelLeftConfiguration.Slot0.kD = - flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD) flywheelRightConfiguration.Slot0.kP = flywheelRightSensor.proportionalVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KP) flywheelRightConfiguration.Slot0.kI = flywheelRightSensor.integralVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KI) flywheelRightConfiguration.Slot0.kD = - flywheelLeftSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD) + flywheelRightSensor.derivativeVelocityGainToRawUnits(FlywheelConstants.PID.REAL_KD) // // flywheelSensor.velocityFeedforwardToRawUnits(FlywheelConstantsConstants.PID.flywheel_KFF) @@ -98,34 +78,20 @@ object FlywheelIOTalon : FlywheelIO { 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 - flywheelLeftTalon.configurator.apply(flywheelRightConfiguration) - flywheelRightTalon.configurator.apply(flywheelLeftConfiguration) + flywheelRightTalon.configurator.apply(flywheelRightConfiguration) - flywheelLeftTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true)) + flywheelRightTalon.setControl(Follower(Constants.Shooter.FLYWHEEL_RIGHT_MOTOR_ID, true)) rightFlywheelStatorCurrentSignal = flywheelRightTalon.statorCurrent rightFlywheelSupplyCurrentSignal = flywheelRightTalon.supplyCurrent rightFlywheelTempSignal = flywheelRightTalon.deviceTemp rightFlywheelDutyCycle = flywheelRightTalon.dutyCycle + motorVoltage = flywheelRightTalon.motorVoltage + motorTorque =flywheelRightTalon.torqueCurrent + - leftFlywheelStatorCurrentSignal = flywheelLeftTalon.statorCurrent - leftFlywheelSupplyCurrentSignal = flywheelLeftTalon.supplyCurrent - leftFlywheelTempSignal = flywheelLeftTalon.deviceTemp - leftFlywheelDutyCycle = flywheelLeftTalon.dutyCycle MotorChecker.add( "Shooter", @@ -172,30 +138,30 @@ object FlywheelIOTalon : FlywheelIO { PIDConfig.kI = flywheelRightSensor.integralVelocityGainToRawUnits(kI) PIDConfig.kD = flywheelRightSensor.derivativeVelocityGainToRawUnits(kD) - flywheelLeftTalon.configurator.apply(PIDConfig) + flywheelRightTalon.configurator.apply(PIDConfig) } override fun setFlywheelVoltage(voltage: ElectricalPotential) { flywheelRightTalon.setVoltage(voltage.inVolts) } + override fun setFlywheelVelocity(velocity: AngularVelocity, feedforward: ElectricalPotential) { - flywheelRightTalon.setControl( - VelocityVoltage(velocity, slot = 0, feedforward = feedforward).velocityVoltagePhoenix6 - ) + FlywheelIOTalon.velocityRequest.setFeedforward(feedforward) + FlywheelIOTalon.velocityRequest.setVelocity(velocity) + FlywheelIOTalon.flywheelRightTalon.setControl(FlywheelIOTalon.velocityRequest.velocityVoltagePhoenix6) } override fun updateInputs(inputs: FlywheelIO.FlywheelIOInputs) { inputs.rightFlywheelVelocity = flywheelRightSensor.velocity - inputs.rightFlywheelAppliedVoltage = rightFlywheelDutyCycle.value.volts + inputs.rightFlywheelAppliedVoltage = motorVoltage.value.volts inputs.rightFlywheelStatorCurrent = rightFlywheelStatorCurrentSignal.value.amps inputs.rightFlywheelSupplyCurrent = rightFlywheelSupplyCurrentSignal.value.amps inputs.rightFlywheelTemperature = rightFlywheelTempSignal.value.celsius + //TODO fix unit for torque + inputs.rightFlywheelTorque = motorTorque.value + inputs.rightFlywheelDutyCycle = rightFlywheelDutyCycle.value.volts + - inputs.leftFlywheelVelocity = flywheelLeftSensor.velocity - inputs.leftFlywheelAppliedVoltage = leftFlywheelDutyCycle.value.volts - inputs.leftFlywheelStatorCurrent = leftFlywheelStatorCurrentSignal.value.amps - inputs.leftFlywheelSupplyCurrent = leftFlywheelSupplyCurrentSignal.value.amps - inputs.leftFlywheelTemperature = leftFlywheelTempSignal.value.celsius } override fun setFlywheelBrakeMode(brake: Boolean) { @@ -206,7 +172,7 @@ object FlywheelIOTalon : FlywheelIO { } else { motorOutputConfig.NeutralMode = NeutralModeValue.Coast } - flywheelLeftTalon.configurator.apply(motorOutputConfig) + flywheelRightTalon.configurator.apply(motorOutputConfig) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 1cedb524..9ec37d20 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023.subsystems.wrist import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.lib.phoenix6.PositionVoltage import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt index b4ace943..f5936e56 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIO.kt @@ -24,12 +24,14 @@ import org.team4099.lib.units.perSecond interface WristIO { class WristIOInputs : LoggableInputs { - var wristPostion = 0.degrees - var wristVelocity = 0.radians.perSecond - var wristAppliedVoltage = 0.volts - var wristStatorCurrent = 0.amps - var wristSupplyCurrent = 0.amps - var wristTemperature = 0.celsius + var wristPostion = 0.0.degrees + var wristVelocity = 0.0.radians.perSecond + var wristAppliedVoltage = 0.0.volts + var wristDutyCycle = 0.0.volts + var wristTorque = 0.0 + var wristStatorCurrent = 0.0.amps + var wristSupplyCurrent = 0.0.amps + var wristTemperature = 0.0.celsius var isSimulated = false diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt index 691784e7..e3749466 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOTalon.kt @@ -30,6 +30,7 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts @@ -43,6 +44,8 @@ object WristIOTalon : WristIO { private val absoluteEncoder: CANcoder = CANcoder(Constants.WRIST.CANCODER_ID) private val absoluteEncoderConfiguration: MagnetSensorConfigs = MagnetSensorConfigs() + var positionRequest = PositionVoltage(-1337.degrees, slot = 0, feedforward = -1337.volts) + private val wristSensor = ctreAngularMechanismSensor( wristTalon, @@ -54,6 +57,8 @@ object WristIOTalon : WristIO { var supplyCurrentSignal: StatusSignal var tempSignal: StatusSignal var dutyCycle: StatusSignal + var motorVoltage : StatusSignal + var motorTorque :StatusSignal init { wristTalon.configurator.apply(TalonFXConfiguration()) @@ -98,6 +103,8 @@ object WristIOTalon : WristIO { supplyCurrentSignal = wristTalon.supplyCurrent tempSignal = wristTalon.deviceTemp dutyCycle = wristTalon.dutyCycle + motorVoltage = wristTalon.motorVoltage + motorTorque = wristTalon.torqueCurrent MotorChecker.add( "Wrist", @@ -130,14 +137,17 @@ object WristIOTalon : WristIO { } override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) { - wristTalon.setControl( - PositionVoltage(position, slot = 0, feedforward = feedforward).positionVoltagePhoenix6 - ) + positionRequest.setFeedforward(feedforward) + positionRequest.setPosition(position) + wristTalon.setControl(positionRequest.positionVoltagePhoenix6) } override fun updateInputs(inputs: WristIO.WristIOInputs) { inputs.wristPostion = wristSensor.position inputs.wristVelocity = wristSensor.velocity - inputs.wristAppliedVoltage = dutyCycle.value.volts + //TODO fix unit for torque + inputs.wristTorque = motorTorque.value + inputs.wristAppliedVoltage = motorVoltage.value.volts + inputs.wristDutyCycle = dutyCycle.value.volts inputs.wristStatorCurrent = statorCurrentSignal.value.amps inputs.wristSupplyCurrent = supplyCurrentSignal.value.amps inputs.wristTemperature = tempSignal.value.celsius