Skip to content

Commit

Permalink
Apply spotless
Browse files Browse the repository at this point in the history
  • Loading branch information
Noga18 committed Jan 15, 2025
1 parent cb5708a commit 98d600a
Show file tree
Hide file tree
Showing 5 changed files with 49 additions and 30 deletions.
1 change: 0 additions & 1 deletion src/main/kotlin/frc/robot/RobotContainer.kt
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

package frc.robot

import com.pathplanner.lib.auto.NamedCommands
Expand Down
1 change: 1 addition & 0 deletions src/main/kotlin/frc/robot/generated/TunerConstants.java
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@

Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@ const val FIRST_STAGE_RATIO = 2.0
const val WEIGHT = 0.0
const val ENCODER_OFSET = 0.0
private val SPROCKET_RADIUS: Distance = Units.Millimeters.of(36.4 / 2)
val ROTATIONS_TO_CENTIMETER = GEAR_RATIO * FIRST_STAGE_RATIO * (SPROCKET_RADIUS.`in`(Units.Centimeter) * 2 * PI)
val ROTATIONS_TO_CENTIMETER =
GEAR_RATIO *
FIRST_STAGE_RATIO *
(SPROCKET_RADIUS.`in`(Units.Centimeter) * 2 * PI)
val GAINS = selectGainsBasedOnMode(Gains(kP = 20.0, kD = 1.0), Gains())

63 changes: 38 additions & 25 deletions src/main/kotlin/frc/robot/subsystems/elevator/ElevatorIOReal.kt
Original file line number Diff line number Diff line change
Expand Up @@ -18,35 +18,41 @@ class ElevatorIOReal : ElevatorIO {
private val motorPositionRequest = PositionVoltage(0.0)

init {
val motorConfig = TalonFXConfiguration().apply {
MotorOutput = MotorOutputConfigs().apply {
NeutralMode = NeutralModeValue.Brake
Inverted = InvertedValue.Clockwise_Positive
val motorConfig =
TalonFXConfiguration().apply {
MotorOutput =
MotorOutputConfigs().apply {
NeutralMode = NeutralModeValue.Brake
Inverted = InvertedValue.Clockwise_Positive
}
Feedback = FeedbackConfigs().apply { RotorToSensorRatio = 1.0 }
Slot0 =
Slot0Configs().apply {
kP = GAINS.kP
kI = GAINS.kI
kD = GAINS.kD
}
CurrentLimits =
CurrentLimitsConfigs().apply {
StatorCurrentLimitEnable = true
SupplyCurrentLimitEnable = true
StatorCurrentLimit = 80.0
SupplyCurrentLimit = 40.0
}
}
Feedback = FeedbackConfigs().apply {
RotorToSensorRatio = 1.0
}
Slot0 = Slot0Configs().apply {
kP = GAINS.kP
kI = GAINS.kI
kD = GAINS.kD
}
CurrentLimits = CurrentLimitsConfigs().apply {
StatorCurrentLimitEnable = true
SupplyCurrentLimitEnable = true
StatorCurrentLimit = 80.0
SupplyCurrentLimit = 40.0
}
}
motor.configurator.apply(motorConfig)

var encoderConfig = CANcoderConfiguration().apply {
MagnetSensor.MagnetOffset = ENCODER_OFSET
}
var encoderConfig =
CANcoderConfiguration().apply {
MagnetSensor.MagnetOffset = ENCODER_OFSET
}
}

override fun setHeight(height: Distance) {
val rotationalPosition = Units.Rotations.of(height.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER)
val rotationalPosition =
Units.Rotations.of(
height.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER
)
motor.setControl(motorPositionRequest.withPosition(rotationalPosition))
}

Expand All @@ -59,9 +65,16 @@ class ElevatorIOReal : ElevatorIO {
}
override fun updateInputs() {
inputs.appliedVoltege = motor.motorVoltage.value
inputs.height = Units.Centimeter.of(motor.position.value.`in`(Units.Rotations) * ROTATIONS_TO_CENTIMETER)
inputs.height =
Units.Centimeter.of(
motor.position.value.`in`(Units.Rotations) *
ROTATIONS_TO_CENTIMETER
)
inputs.noOffsetAbsoluteEncoderPosition = encoder.absolutePosition.value
inputs.absoluteEncoderHeight =
Units.Centimeter.of(encoder.position.value.`in`(Units.Rotations) * ROTATIONS_TO_CENTIMETER)
Units.Centimeter.of(
encoder.position.value.`in`(Units.Rotations) *
ROTATIONS_TO_CENTIMETER
)
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,10 @@ class ElevatorIOSim : ElevatorIO {
)

override fun setHeight(position: Distance) {
val rotationalPosition = Units.Rotations.of(position.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER)
val rotationalPosition =
Units.Rotations.of(
position.`in`(Units.Centimeter) / ROTATIONS_TO_CENTIMETER
)
motor.setControl(motorPosititonRequest.withPosition(rotationalPosition))
}
override fun setPower(percentOutput: Double) {
Expand All @@ -36,6 +39,7 @@ class ElevatorIOSim : ElevatorIO {

override fun updateInputs() {
inputs.appliedVoltege = Units.Volts.of(motor.appliedVoltage)
inputs.height = Units.Centimeter.of(motor.position * ROTATIONS_TO_CENTIMETER)
inputs.height =
Units.Centimeter.of(motor.position * ROTATIONS_TO_CENTIMETER)
}
}

0 comments on commit 98d600a

Please sign in to comment.