From ea7dda10179be733082eb45807ef7e8b080e72b3 Mon Sep 17 00:00:00 2001 From: taimorioka Date: Tue, 7 Jan 2025 17:29:15 -0800 Subject: [PATCH 1/6] working, still not done --- .../frc2025/subsystems/Elevator/Elevator.kt | 54 ++++++++++++++++ .../frc2025/subsystems/Elevator/ElevatorIO.kt | 63 +++++++++++++++++++ 2 files changed, 117 insertions(+) create mode 100644 src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt create mode 100644 src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt new file mode 100644 index 0000000..d2dc64b --- /dev/null +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt @@ -0,0 +1,54 @@ +import com.frcteam3636.frc2025.Robot +import edu.wpi.first.units.Units.Meters +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.wpilibj.Timer +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Subsystem +import org.littletonrobotics.junction.Logger + +private const val SECONDS_BETWEEN_ELEVATOR_UPDATES = 0.5 + +object Elevator: Subsystem { + private val io: ElevatorIO = when (Robot.model) { + Robot.Model.SIMULATION -> ElevatorIOSim() + Robot.Model.COMPETITION -> ElevatorIOReal() + Robot.Model.PROTOTYPE -> ElevatorIOReal() + } + + var inputs = LoggedElevatorInputs() + + private var timer = Timer().apply { + start() + } + + override fun periodic() { + io.updateInputs(inputs) + Logger.processInputs("Elevator", inputs) + if (timer.advanceIfElapsed(SECONDS_BETWEEN_ELEVATOR_UPDATES) && inputs.absoluteEncoderConnected){ + io.updateHeight(inputs.absoluteEncoderHeight) + } + } + + fun setTargetHeight(position: Position): Command = + startEnd({ + io.runToHeight(position.height) + }, { + io.runToHeight(inputs.height) + })!! + +// fun sysIdQuasistatic(direction: Direction) = +// sysID.quasistatic(direction)!! +// +// fun sysIdDynamic(direction: Direction) = +// sysID.dynamic(direction)!! + + enum class Position(val height: Distance) { + Stowed(Meters.of(0.0)), + Trough(Meters.of(0.0)), + LowBar(Meters.of(0.0)), + MidBar(Meters.of(0.0)), + HighBar(Meters.of(0.0)), + LowAlgae(Meters.of(0.0)), + HighAlgae(Meters.of(0.0)), + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt new file mode 100644 index 0000000..39c08eb --- /dev/null +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt @@ -0,0 +1,63 @@ +import com.frcteam3636.frc2025.CTREDeviceId +import com.frcteam3636.frc2025.TalonFX +import edu.wpi.first.units.Units.* +import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.Voltage +import edu.wpi.first.wpilibj.DigitalInput +import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.team9432.annotation.Logged + +@Logged + +open class ElevatorInputs { + var height = Meters.zero()!! + var absoluteEncoderHeight = Meters.zero()!! + var current = Volts.zero()!! + var velocity = MetersPerSecond.zero()!! + var absoluteEncoderConnected = false +} + +interface ElevatorIO{ + + fun updateInputs(inputs: ElevatorInputs) + + fun runToHeight(height: Distance) + + fun setVoltage(volts: Voltage) + + fun updateHeight(height: Distance) +} + +class ElevatorIOReal: ElevatorIO{ + + private val elevatorMotor = TalonFX(CTREDeviceId.LeftArmMotor) + + private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + + override fun updateInputs(inputs: ElevatorInputs) { + inputs.absoluteEncoderHeight = Meters.of(absoluteEncoder.absolutePosition) + inputs.absoluteEncoderConnected = absoluteEncoder.isConnected + inputs.height = (METERS_PER_ROTATION * elevatorMotor.position.value) as Distance + } + + override fun runToHeight(height: Distance) { + + } + + override fun setVoltage(volts: Voltage){ + + } + + override fun updateHeight(height: Distance) { + + } + + init { + + } + + internal companion object Constants { + val METERS_PER_ROTATION = Meters.per(Rotation).of(0.0)!! + } + +} \ No newline at end of file From 4c982e594641e5059c00b0f9c38c87fef4876481 Mon Sep 17 00:00:00 2001 From: Tai Morioka <148292625+Taimorioka@users.noreply.github.com> Date: Wed, 8 Jan 2025 15:41:28 -0800 Subject: [PATCH 2/6] small changes --- .../frc2025/subsystems/Elevator/ElevatorIO.kt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt index 39c08eb..e19b0b9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt @@ -30,7 +30,9 @@ interface ElevatorIO{ class ElevatorIOReal: ElevatorIO{ - private val elevatorMotor = TalonFX(CTREDeviceId.LeftArmMotor) + private val elevatorMotor = TalonFX(CTREDeviceId.LeftArmMotor).apply { + //pid + } private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) @@ -38,10 +40,13 @@ class ElevatorIOReal: ElevatorIO{ inputs.absoluteEncoderHeight = Meters.of(absoluteEncoder.absolutePosition) inputs.absoluteEncoderConnected = absoluteEncoder.isConnected inputs.height = (METERS_PER_ROTATION * elevatorMotor.position.value) as Distance + inputs.velocity = RotationsPerSecond.of(elevatorMotor.velocity.value) + inputs.current = Volts.of(elevatorMotor.motorVoltage.value) } override fun runToHeight(height: Distance) { - + Logger.recordOutput("Elevator/Height Setpoint", height) + //use pid to do this } override fun setVoltage(volts: Voltage){ From f2f8529024f5ddcd46352f8c86b9bfab13355571 Mon Sep 17 00:00:00 2001 From: taimorioka Date: Wed, 8 Jan 2025 16:28:52 -0800 Subject: [PATCH 3/6] working --- .../kotlin/com/frcteam3636/frc2025/CAN.kt | 1 + .../frc2025/subsystems/Elevator/ElevatorIO.kt | 71 +++++++++++++++---- 2 files changed, 58 insertions(+), 14 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt index 305a5de..769f3a9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt @@ -36,6 +36,7 @@ enum class CTREDeviceId(val num: Int, val bus: String) { BackRightDrivingMotor(3, "*"), FrontRightDrivingMotor(4, "*"), PigeonGyro(20, "*"), + ElevatorMotor(11, "*"), } fun TalonFX(id: CTREDeviceId) = TalonFX(id.num, id.bus) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt index e19b0b9..8904c3f 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt @@ -1,10 +1,22 @@ +import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC +import com.ctre.phoenix6.controls.VoltageOut +import com.ctre.phoenix6.signals.InvertedValue +import com.ctre.phoenix6.signals.NeutralModeValue import com.frcteam3636.frc2025.CTREDeviceId import com.frcteam3636.frc2025.TalonFX +import com.frcteam3636.frc2025.utils.math.MotorFFGains +import com.frcteam3636.frc2025.utils.math.PIDGains +import com.frcteam3636.frc2025.utils.math.motorFFGains +import com.frcteam3636.frc2025.utils.math.pidGains import edu.wpi.first.units.Units.* +import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance +import edu.wpi.first.units.measure.LinearVelocity import edu.wpi.first.units.measure.Voltage import edu.wpi.first.wpilibj.DigitalInput import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.littletonrobotics.junction.Logger import org.team9432.annotation.Logged @Logged @@ -28,41 +40,72 @@ interface ElevatorIO{ fun updateHeight(height: Distance) } -class ElevatorIOReal: ElevatorIO{ - - private val elevatorMotor = TalonFX(CTREDeviceId.LeftArmMotor).apply { - //pid +class ElevatorIOReal: ElevatorIO { + + private val elevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply { + val config = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + } + + Feedback.apply { + SensorToMechanismRatio = GEAR_RATIO + FeedbackRotorOffset = 0.0 + } + + Slot0.apply { + pidGains = PID_GAIN + motorFFGains = FF_GAINS + kG = GRAVITY_GAIN + } + + MotionMagic.apply { + MotionMagicCruiseVelocity = PROFILE_VELOCITY + MotionMagicAcceleration = PROFILE_ACCELERATION + MotionMagicJerk = PROFILE_JERK + } + } + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + configurator.apply(config) } private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) override fun updateInputs(inputs: ElevatorInputs) { - inputs.absoluteEncoderHeight = Meters.of(absoluteEncoder.absolutePosition) + inputs.absoluteEncoderHeight = (METERS_PER_ROTATION * Rotations.of(absoluteEncoder.get())) as Distance inputs.absoluteEncoderConnected = absoluteEncoder.isConnected inputs.height = (METERS_PER_ROTATION * elevatorMotor.position.value) as Distance - inputs.velocity = RotationsPerSecond.of(elevatorMotor.velocity.value) - inputs.current = Volts.of(elevatorMotor.motorVoltage.value) + inputs.velocity = (METERS_PER_ROTATION * elevatorMotor.velocity.value) as LinearVelocity + inputs.current = elevatorMotor.motorVoltage.value } override fun runToHeight(height: Distance) { Logger.recordOutput("Elevator/Height Setpoint", height) - //use pid to do this + + var desiredAngle = (height / METERS_PER_ROTATION) as Angle + var controlRequest = MotionMagicTorqueCurrentFOC(desiredAngle) + elevatorMotor.setControl(controlRequest) } override fun setVoltage(volts: Voltage){ - + assert(volts in Volts.of(-12.0)..Volts.of(12.0)) + val control = VoltageOut(volts.`in`(Volts)) + elevatorMotor.setControl(control) } override fun updateHeight(height: Distance) { - - } - - init { - + elevatorMotor.setPosition(height.`in`(Meters)) } internal companion object Constants { val METERS_PER_ROTATION = Meters.per(Rotation).of(0.0)!! + private const val GEAR_RATIO = 0.0 + val PID_GAINS = PIDGains(0.0, 0.0, 0.0) + val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0) + private const val GRAVITY_GAIN = 0.0 + private const val PROFILE_ACCELERATION = 0.0 + private const val PROFILE_JERK = 0.0 + private const val PROFILE_VELOCITY = 0.0 } } \ No newline at end of file From a439dc4724e60bf83b0d576c8eccc52f643b78df Mon Sep 17 00:00:00 2001 From: taiMorioka Date: Thu, 9 Jan 2025 14:48:58 -0800 Subject: [PATCH 4/6] Should be done except for height presets when setting height --- .../kotlin/org/team9432/annotation/LogTableUtils.kt | 2 +- .../subsystems/{Elevator => elevator}/Elevator.kt | 6 ++++-- .../subsystems/{Elevator => elevator}/ElevatorIO.kt | 10 +++++----- 3 files changed, 10 insertions(+), 8 deletions(-) rename src/main/kotlin/com/frcteam3636/frc2025/subsystems/{Elevator => elevator}/Elevator.kt (91%) rename src/main/kotlin/com/frcteam3636/frc2025/subsystems/{Elevator => elevator}/ElevatorIO.kt (95%) diff --git a/annotation/src/main/kotlin/org/team9432/annotation/LogTableUtils.kt b/annotation/src/main/kotlin/org/team9432/annotation/LogTableUtils.kt index 65e8c63..1933bd9 100644 --- a/annotation/src/main/kotlin/org/team9432/annotation/LogTableUtils.kt +++ b/annotation/src/main/kotlin/org/team9432/annotation/LogTableUtils.kt @@ -80,7 +80,7 @@ object LogTableUtils { fun > LogTable.kGet(key: String, defaultValue: E): E = get(key, defaultValue) - fun LogTable.kGet(key: String, defaultValue: Measure): Measure = get(key, defaultValue) + fun , U: Unit> LogTable.kGet(key: String, defaultValue: M): M = get(key, defaultValue) fun LogTable.kGet(key: String, defaultValue: BooleanArray): BooleanArray = get(key, defaultValue) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt similarity index 91% rename from src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt rename to src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt index d2dc64b..5625f4e 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/Elevator.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt @@ -1,3 +1,5 @@ +package com.frcteam3636.frc2025.subsystems.elevator + import com.frcteam3636.frc2025.Robot import edu.wpi.first.units.Units.Meters import edu.wpi.first.units.measure.Distance @@ -10,9 +12,9 @@ private const val SECONDS_BETWEEN_ELEVATOR_UPDATES = 0.5 object Elevator: Subsystem { private val io: ElevatorIO = when (Robot.model) { - Robot.Model.SIMULATION -> ElevatorIOSim() + Robot.Model.SIMULATION -> TODO() Robot.Model.COMPETITION -> ElevatorIOReal() - Robot.Model.PROTOTYPE -> ElevatorIOReal() + Robot.Model.PROTOTYPE -> TODO() } var inputs = LoggedElevatorInputs() diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt similarity index 95% rename from src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt rename to src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt index 8904c3f..2306993 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/Elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt @@ -1,3 +1,5 @@ +package com.frcteam3636.frc2025.subsystems.elevator + import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.controls.VoltageOut @@ -30,7 +32,6 @@ open class ElevatorInputs { } interface ElevatorIO{ - fun updateInputs(inputs: ElevatorInputs) fun runToHeight(height: Distance) @@ -54,7 +55,7 @@ class ElevatorIOReal: ElevatorIO { } Slot0.apply { - pidGains = PID_GAIN + pidGains = PID_GAINS motorFFGains = FF_GAINS kG = GRAVITY_GAIN } @@ -81,9 +82,8 @@ class ElevatorIOReal: ElevatorIO { override fun runToHeight(height: Distance) { Logger.recordOutput("Elevator/Height Setpoint", height) - - var desiredAngle = (height / METERS_PER_ROTATION) as Angle - var controlRequest = MotionMagicTorqueCurrentFOC(desiredAngle) + var desiredMotorAngle = (height / METERS_PER_ROTATION) as Angle + var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle) elevatorMotor.setControl(controlRequest) } From d16ab665acddabc26ac05660ebe5ca2fed736dde Mon Sep 17 00:00:00 2001 From: taiMorioka Date: Mon, 13 Jan 2025 15:24:20 -0800 Subject: [PATCH 5/6] changed to two motors and cancoder. --- .../kotlin/com/frcteam3636/frc2025/CAN.kt | 5 +- .../frc2025/subsystems/elevator/Elevator.kt | 10 -- .../frc2025/subsystems/elevator/ElevatorIO.kt | 94 +++++++++++-------- 3 files changed, 57 insertions(+), 52 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt index 769f3a9..47ff5bd 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/CAN.kt @@ -1,5 +1,6 @@ package com.frcteam3636.frc2025 +import com.ctre.phoenix6.hardware.CANcoder import com.ctre.phoenix6.hardware.Pigeon2 import com.ctre.phoenix6.hardware.TalonFX import com.revrobotics.spark.SparkMax @@ -35,9 +36,11 @@ enum class CTREDeviceId(val num: Int, val bus: String) { BackLeftDrivingMotor(2, "*"), BackRightDrivingMotor(3, "*"), FrontRightDrivingMotor(4, "*"), - PigeonGyro(20, "*"), ElevatorMotor(11, "*"), + PigeonGyro(20, "*"), + ElevatorEncoder(30, "*"), } +fun CANcoder(id: CTREDeviceId) = CANcoder(id.num, id.bus) fun TalonFX(id: CTREDeviceId) = TalonFX(id.num, id.bus) fun Pigeon2(id: CTREDeviceId) = Pigeon2(id.num, id.bus) \ No newline at end of file diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt index 5625f4e..65b77c9 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/Elevator.kt @@ -3,13 +3,10 @@ package com.frcteam3636.frc2025.subsystems.elevator import com.frcteam3636.frc2025.Robot import edu.wpi.first.units.Units.Meters import edu.wpi.first.units.measure.Distance -import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger -private const val SECONDS_BETWEEN_ELEVATOR_UPDATES = 0.5 - object Elevator: Subsystem { private val io: ElevatorIO = when (Robot.model) { Robot.Model.SIMULATION -> TODO() @@ -19,16 +16,9 @@ object Elevator: Subsystem { var inputs = LoggedElevatorInputs() - private var timer = Timer().apply { - start() - } - override fun periodic() { io.updateInputs(inputs) Logger.processInputs("Elevator", inputs) - if (timer.advanceIfElapsed(SECONDS_BETWEEN_ELEVATOR_UPDATES) && inputs.absoluteEncoderConnected){ - io.updateHeight(inputs.absoluteEncoderHeight) - } } fun setTargetHeight(position: Position): Command = diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt index 2306993..2232ef2 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt @@ -1,12 +1,16 @@ package com.frcteam3636.frc2025.subsystems.elevator +import com.ctre.phoenix6.configs.CANcoderConfiguration import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC import com.ctre.phoenix6.controls.VoltageOut +import com.ctre.phoenix6.signals.FeedbackSensorSourceValue import com.ctre.phoenix6.signals.InvertedValue import com.ctre.phoenix6.signals.NeutralModeValue +import com.ctre.phoenix6.signals.SensorDirectionValue import com.frcteam3636.frc2025.CTREDeviceId import com.frcteam3636.frc2025.TalonFX +import com.frcteam3636.frc2025.CANcoder import com.frcteam3636.frc2025.utils.math.MotorFFGains import com.frcteam3636.frc2025.utils.math.PIDGains import com.frcteam3636.frc2025.utils.math.motorFFGains @@ -16,8 +20,6 @@ import edu.wpi.first.units.measure.Angle import edu.wpi.first.units.measure.Distance import edu.wpi.first.units.measure.LinearVelocity import edu.wpi.first.units.measure.Voltage -import edu.wpi.first.wpilibj.DigitalInput -import edu.wpi.first.wpilibj.DutyCycleEncoder import org.littletonrobotics.junction.Logger import org.team9432.annotation.Logged @@ -25,10 +27,9 @@ import org.team9432.annotation.Logged open class ElevatorInputs { var height = Meters.zero()!! - var absoluteEncoderHeight = Meters.zero()!! - var current = Volts.zero()!! + var rightCurrent = Volts.zero()!! + var leftCurrent = Volts.zero()!! var velocity = MetersPerSecond.zero()!! - var absoluteEncoderConnected = false } interface ElevatorIO{ @@ -38,63 +39,74 @@ interface ElevatorIO{ fun setVoltage(volts: Voltage) - fun updateHeight(height: Distance) } class ElevatorIOReal: ElevatorIO { - private val elevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply { - val config = TalonFXConfiguration().apply { - MotorOutput.apply { - NeutralMode = NeutralModeValue.Brake + private val encoder = CANcoder(CTREDeviceId.ElevatorEncoder).apply { + val config = CANcoderConfiguration().apply { + MagnetSensor.apply { + withAbsoluteSensorDiscontinuityPoint(Rotations.one()) + SensorDirection = SensorDirectionValue.Clockwise_Positive } + } + configurator.apply(config) + } - Feedback.apply { - SensorToMechanismRatio = GEAR_RATIO - FeedbackRotorOffset = 0.0 - } + val config = TalonFXConfiguration().apply { + MotorOutput.apply { + NeutralMode = NeutralModeValue.Brake + } - Slot0.apply { - pidGains = PID_GAINS - motorFFGains = FF_GAINS - kG = GRAVITY_GAIN - } + Feedback.apply { + SensorToMechanismRatio = GEAR_RATIO + FeedbackRemoteSensorID = CTREDeviceId.ElevatorEncoder.num + FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder + } - MotionMagic.apply { - MotionMagicCruiseVelocity = PROFILE_VELOCITY - MotionMagicAcceleration = PROFILE_ACCELERATION - MotionMagicJerk = PROFILE_JERK - } + Slot0.apply { + pidGains = PID_GAINS + motorFFGains = FF_GAINS + kG = GRAVITY_GAIN } - config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + + MotionMagic.apply { + MotionMagicCruiseVelocity = PROFILE_VELOCITY + MotionMagicAcceleration = PROFILE_ACCELERATION + MotionMagicJerk = PROFILE_JERK + } + } + + private val rightElevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply { + config.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive configurator.apply(config) } - private val absoluteEncoder = DutyCycleEncoder(DigitalInput(0)) + private val leftElevatorMotor = TalonFX(CTREDeviceId.ElevatorMotor).apply { + config.MotorOutput.Inverted = InvertedValue.Clockwise_Positive + configurator.apply(config) + } override fun updateInputs(inputs: ElevatorInputs) { - inputs.absoluteEncoderHeight = (METERS_PER_ROTATION * Rotations.of(absoluteEncoder.get())) as Distance - inputs.absoluteEncoderConnected = absoluteEncoder.isConnected - inputs.height = (METERS_PER_ROTATION * elevatorMotor.position.value) as Distance - inputs.velocity = (METERS_PER_ROTATION * elevatorMotor.velocity.value) as LinearVelocity - inputs.current = elevatorMotor.motorVoltage.value + inputs.height = (METERS_PER_ROTATION * encoder.position.value) as Distance + inputs.velocity = (METERS_PER_ROTATION * encoder.velocity.value) as LinearVelocity + inputs.rightCurrent = rightElevatorMotor.motorVoltage.value + inputs.leftCurrent = leftElevatorMotor.motorVoltage.value } override fun runToHeight(height: Distance) { - Logger.recordOutput("Elevator/Height Setpoint", height) - var desiredMotorAngle = (height / METERS_PER_ROTATION) as Angle - var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle) - elevatorMotor.setControl(controlRequest) + Logger.recordOutput("Elevator/Height Setpoint", height) + var desiredMotorAngle = (height / METERS_PER_ROTATION) as Angle + var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle) + rightElevatorMotor.setControl(controlRequest) + leftElevatorMotor.setControl(controlRequest) } override fun setVoltage(volts: Voltage){ assert(volts in Volts.of(-12.0)..Volts.of(12.0)) - val control = VoltageOut(volts.`in`(Volts)) - elevatorMotor.setControl(control) - } - - override fun updateHeight(height: Distance) { - elevatorMotor.setPosition(height.`in`(Meters)) + val controlRequest = VoltageOut(volts.`in`(Volts)) + rightElevatorMotor.setControl(controlRequest) + leftElevatorMotor.setControl(controlRequest) } internal companion object Constants { From 82505a1ac6b0f259125dfbb42b551aede44d80d9 Mon Sep 17 00:00:00 2001 From: doinkythederp Date: Tue, 14 Jan 2025 08:41:26 -0800 Subject: [PATCH 6/6] refactor: remove specific units from angular-linear conversion constant name --- .../frc2025/subsystems/elevator/ElevatorIO.kt | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt index 2232ef2..4c003d7 100644 --- a/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/frcteam3636/frc2025/subsystems/elevator/ElevatorIO.kt @@ -24,7 +24,6 @@ import org.littletonrobotics.junction.Logger import org.team9432.annotation.Logged @Logged - open class ElevatorInputs { var height = Meters.zero()!! var rightCurrent = Volts.zero()!! @@ -88,15 +87,15 @@ class ElevatorIOReal: ElevatorIO { } override fun updateInputs(inputs: ElevatorInputs) { - inputs.height = (METERS_PER_ROTATION * encoder.position.value) as Distance - inputs.velocity = (METERS_PER_ROTATION * encoder.velocity.value) as LinearVelocity + inputs.height = (DISTANCE_PER_TURN * encoder.position.value) as Distance + inputs.velocity = (DISTANCE_PER_TURN * encoder.velocity.value) as LinearVelocity inputs.rightCurrent = rightElevatorMotor.motorVoltage.value inputs.leftCurrent = leftElevatorMotor.motorVoltage.value } override fun runToHeight(height: Distance) { Logger.recordOutput("Elevator/Height Setpoint", height) - var desiredMotorAngle = (height / METERS_PER_ROTATION) as Angle + var desiredMotorAngle = (height / DISTANCE_PER_TURN) as Angle var controlRequest = MotionMagicTorqueCurrentFOC(desiredMotorAngle) rightElevatorMotor.setControl(controlRequest) leftElevatorMotor.setControl(controlRequest) @@ -110,7 +109,7 @@ class ElevatorIOReal: ElevatorIO { } internal companion object Constants { - val METERS_PER_ROTATION = Meters.per(Rotation).of(0.0)!! + val DISTANCE_PER_TURN = Meters.per(Rotation).of(0.0)!! private const val GEAR_RATIO = 0.0 val PID_GAINS = PIDGains(0.0, 0.0, 0.0) val FF_GAINS = MotorFFGains(0.0, 0.0, 0.0) @@ -120,4 +119,4 @@ class ElevatorIOReal: ElevatorIO { private const val PROFILE_VELOCITY = 0.0 } -} \ No newline at end of file +}