Skip to content

Commit d2cb2d3

Browse files
Augustine-DoanAugustine-Doan
committed
feat(elevator): created subsystem
added function to robotparameters created enum elevatorstate
1 parent 7d19d8b commit d2cb2d3

3 files changed

Lines changed: 169 additions & 0 deletions

File tree

‎src/main/java/frc/robot/subsystems/Elevator.kt‎

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
package frc.robot.subsystems
2+
3+
import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs
4+
import com.ctre.phoenix6.configs.CurrentLimitsConfigs
5+
import com.ctre.phoenix6.configs.MotionMagicConfigs
6+
import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs
7+
import com.ctre.phoenix6.configs.TalonFXConfiguration
8+
import com.ctre.phoenix6.controls.DutyCycleOut
9+
import com.ctre.phoenix6.controls.MotionMagicVoltage
10+
import com.ctre.phoenix6.controls.PositionDutyCycle
11+
import com.ctre.phoenix6.controls.PositionVoltage
12+
import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC
13+
import com.ctre.phoenix6.controls.VoltageOut
14+
import com.ctre.phoenix6.hardware.TalonFX
15+
import com.ctre.phoenix6.signals.InvertedValue
16+
import com.ctre.phoenix6.signals.NeutralModeValue
17+
import edu.wpi.first.wpilibj2.command.SubsystemBase
18+
import frc.robot.utils.ElevatorParameters.ELEVATOR_MAGIC_PINGU
19+
import frc.robot.utils.ElevatorParameters.ELEVATOR_PINGU
20+
import frc.robot.utils.ElevatorParameters.ELEVATOR_SOFT_LIMIT_DOWN
21+
import frc.robot.utils.ElevatorParameters.ELEVATOR_SOFT_LIMIT_UP
22+
import frc.robot.utils.emu.ElevatorState
23+
import org.littletonrobotics.junction.networktables.LoggedNetworkNumber
24+
import xyz.malefic.frc.pingu.AlertPingu.add
25+
26+
object Elevator : SubsystemBase() {
27+
private val elevatorMotor: TalonFX = TalonFX(0)
28+
private val elevatorConfigs: TalonFXConfiguration = TalonFXConfiguration()
29+
private val voltagePos: PositionVoltage = PositionVoltage(0.0)
30+
private val posRequest: PositionDutyCycle
31+
private val velocityRequest: VelocityTorqueCurrentFOC
32+
private val voltageOut: VoltageOut
33+
private val motionMagicVoltage: MotionMagicVoltage
34+
private val cycleOut: DutyCycleOut
35+
private var motionMagicConfigs: MotionMagicConfigs
36+
private var elevatorP: LoggedNetworkNumber? = null
37+
private var elevatorI: LoggedNetworkNumber? = null
38+
private var elevatorD: LoggedNetworkNumber? = null
39+
private var elevatorV: LoggedNetworkNumber? = null
40+
private var elevatorS: LoggedNetworkNumber? = null
41+
private var elevatorG: LoggedNetworkNumber? = null
42+
private var cruiseV: LoggedNetworkNumber? = null
43+
private var acc: LoggedNetworkNumber? = null
44+
private var jerk: LoggedNetworkNumber? = null
45+
46+
init {
47+
elevatorConfigs.MotorOutput.NeutralMode = NeutralModeValue.Brake
48+
elevatorConfigs.Slot0.kP = ELEVATOR_PINGU.p
49+
elevatorConfigs.Slot0.kI = ELEVATOR_PINGU.i
50+
elevatorConfigs.Slot0.kD = ELEVATOR_PINGU.d
51+
elevatorConfigs.Slot0.kV = ELEVATOR_PINGU.v!!
52+
elevatorConfigs.Slot0.kS = ELEVATOR_PINGU.s!!
53+
elevatorMotor.configurator.apply(elevatorConfigs)
54+
val elevatorMotorCurrentConfig = CurrentLimitsConfigs()
55+
val elevatorMotorRampConfig = ClosedLoopRampsConfigs()
56+
val elevatorSoftLimitConfig = SoftwareLimitSwitchConfigs()
57+
elevatorMotorCurrentConfig.SupplyCurrentLimit = 40.79
58+
elevatorMotorCurrentConfig.SupplyCurrentLimitEnable = true
59+
elevatorMotor.configurator.apply(elevatorMotorCurrentConfig)
60+
elevatorMotorRampConfig.DutyCycleClosedLoopRampPeriod = 0.0
61+
elevatorMotor.configurator.apply(elevatorMotorRampConfig)
62+
elevatorSoftLimitConfig.ForwardSoftLimitEnable = true
63+
elevatorSoftLimitConfig.ReverseSoftLimitEnable = true
64+
elevatorSoftLimitConfig.ForwardSoftLimitThreshold = ELEVATOR_SOFT_LIMIT_UP
65+
elevatorSoftLimitConfig.ReverseSoftLimitThreshold = ELEVATOR_SOFT_LIMIT_DOWN
66+
elevatorConfigs.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive
67+
elevatorConfigs.SoftwareLimitSwitch = elevatorSoftLimitConfig
68+
elevatorConfigs.MotorOutput.DutyCycleNeutralDeadband = 0.1
69+
elevatorMotor.configurator.apply(elevatorSoftLimitConfig)
70+
velocityRequest = VelocityTorqueCurrentFOC(0.0)
71+
posRequest = PositionDutyCycle(0.0)
72+
voltageOut = VoltageOut(0.0)
73+
motionMagicVoltage = MotionMagicVoltage(0.0)
74+
cycleOut = DutyCycleOut(0.0)
75+
motionMagicConfigs = elevatorConfigs.MotionMagic
76+
motionMagicConfigs.MotionMagicCruiseVelocity = ELEVATOR_MAGIC_PINGU.velocity
77+
motionMagicConfigs.MotionMagicAcceleration = ELEVATOR_MAGIC_PINGU.acceleration
78+
motionMagicConfigs.MotionMagicJerk = ELEVATOR_MAGIC_PINGU.jerk
79+
motionMagicVoltage.Slot = 0
80+
velocityRequest.OverrideCoastDurNeutral = false
81+
82+
voltageOut.OverrideBrakeDurNeutral = false
83+
voltageOut.EnableFOC = true
84+
85+
cycleOut.EnableFOC = false
86+
elevatorMotor.setPosition(0.0)
87+
88+
elevatorMotor.configurator.apply(elevatorConfigs)
89+
90+
elevatorMotor.configurator.apply(motionMagicConfigs)
91+
92+
add(elevatorMotor, "elevator")
93+
initializeLoggedNetworkPID()
94+
}
95+
96+
fun initializeLoggedNetworkPID() {
97+
elevatorP =
98+
LoggedNetworkNumber("Tuning/elevator/elevator P", elevatorConfigs.Slot0.kP)
99+
elevatorI =
100+
LoggedNetworkNumber("Tuning/elevator/elevator I", elevatorConfigs.Slot0.kI)
101+
elevatorD =
102+
LoggedNetworkNumber("Tuning/elevator/elevator D", elevatorConfigs.Slot0.kD)
103+
elevatorV =
104+
LoggedNetworkNumber("Tuning/elevator/elevator V", elevatorConfigs.Slot0.kV)
105+
elevatorS =
106+
LoggedNetworkNumber("Tuning/elevator/elevator S", elevatorConfigs.Slot0.kS)
107+
elevatorG =
108+
LoggedNetworkNumber("Tuning/elevator/elevator G", elevatorConfigs.Slot0.kG)
109+
110+
cruiseV =
111+
LoggedNetworkNumber(
112+
"Tuning/elevator/MM Cruise Velocity",
113+
motionMagicConfigs.MotionMagicCruiseVelocity,
114+
)
115+
acc =
116+
LoggedNetworkNumber(
117+
"Tuning/elevator/MM Acceleration",
118+
motionMagicConfigs.MotionMagicAcceleration,
119+
)
120+
jerk =
121+
LoggedNetworkNumber(
122+
"Tuning/elevator/MM Jerk",
123+
@Suppress("ktlint:standard:max-line-length")
124+
motionMagicConfigs.MotionMagicJerk,
125+
)
126+
}
127+
128+
fun elevatorMovement(state: ElevatorState) {
129+
elevatorMotor.setControl(voltagePos.withPosition(state.pos))
130+
}
131+
}

‎src/main/java/frc/robot/utils/RobotParameters.kt‎

Lines changed: 24 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,8 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController
77
import edu.wpi.first.math.controller.PIDController
88
import edu.wpi.first.math.geometry.Translation2d
99
import edu.wpi.first.math.kinematics.SwerveDriveKinematics
10+
import xyz.malefic.frc.pingu.MagicPingu
11+
import xyz.malefic.frc.pingu.Pingu
1012

1113
/** Class containing global values for the robot. */
1214
object RobotParameters {
@@ -158,3 +160,25 @@ object RobotParameters {
158160
const val OFFSET_TOWARD_MID_RIGHT: Double = 15.0
159161
}
160162
}
163+
164+
object ElevatorParameters {
165+
@JvmField
166+
val ELEVATOR_PINGU: Pingu = Pingu(5.0, 0.0, 0.0, 0.35, 0.5199, 0.42) // g could be 0.42
167+
168+
// MM ↓
169+
170+
@JvmField
171+
val ELEVATOR_MAGIC_PINGU = MagicPingu(90.0, 180.0, 0.0)
172+
173+
@Suppress("ktlint:standard:property-naming")
174+
const val ELEVATOR_SOFT_LIMIT_DOWN: Double = 0.0
175+
176+
@Suppress("ktlint:standard:property-naming")
177+
const val ELEVATOR_SOFT_LIMIT_UP: Double = 65.0
178+
179+
@JvmField
180+
var elevatorUp: Boolean = true
181+
182+
@JvmField
183+
var isSoftLimitEnabled: Boolean = false
184+
}

‎src/main/java/frc/robot/utils/emu/ElevatorState.kt‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
@file:Suppress("ktlint:standard:filename")
2+
3+
package frc.robot.utils.emu
4+
5+
// Enum class for the position of Elevator
6+
enum class ElevatorState(
7+
@JvmField val pos: Double,
8+
) {
9+
UP(65.0),
10+
UPPER_UP(48.75),
11+
MIDDLE(32.5),
12+
LOWER_DOWN(16.25),
13+
DOWN(0.0),
14+
}

0 commit comments

Comments
 (0)