Skip to content

Commit 370cd35

Browse files
BrodyKarrBrodyKarr
authored
Updated the elevator subsystem🦟™️ (#32)
1 parent 6871d9a commit 370cd35

File tree

4 files changed

+156
-18
lines changed

4 files changed

+156
-18
lines changed

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 72 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -10,9 +10,11 @@
1010

1111
import com.ctre.phoenix6.configs.CANcoderConfiguration;
1212
import com.ctre.phoenix6.configs.TalonFXConfiguration;
13+
import com.ctre.phoenix6.signals.GravityTypeValue;
1314
import com.ctre.phoenix6.signals.InvertedValue;
1415
import com.ctre.phoenix6.signals.NeutralModeValue;
1516
import com.ctre.phoenix6.signals.SensorDirectionValue;
17+
import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
1618
import com.frcteam3255.components.swerve.SN_SwerveConstants;
1719
import com.pathplanner.lib.config.ModuleConfig;
1820
import com.pathplanner.lib.config.PIDConstants;
@@ -34,7 +36,8 @@
3436
import edu.wpi.first.units.measure.Distance;
3537
import edu.wpi.first.units.measure.LinearVelocity;
3638
import edu.wpi.first.units.measure.Mass;
37-
import edu.wpi.first.units.Unit;
39+
import edu.wpi.first.units.measure.Time;
40+
import edu.wpi.first.units.measure.Voltage;
3841
import edu.wpi.first.units.Units;
3942
import edu.wpi.first.wpilibj.DriverStation;
4043
import edu.wpi.first.wpilibj.DriverStation.Alliance;
@@ -246,10 +249,75 @@ public static boolean isRedAlliance() {
246249
public static class constElevator {
247250

248251
public static final Distance CORAL_L1_HEIGHT = Units.Inches.of(0);
249-
public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(2);
250-
public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(3);
251-
public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(4);
252+
public static final Distance CORAL_L2_HEIGHT = Units.Inches.of(0);
253+
public static final Distance CORAL_L3_HEIGHT = Units.Inches.of(0);
254+
public static final Distance CORAL_L4_HEIGHT = Units.Inches.of(0);
255+
public static final Distance ALGAE_NET_HEIGHT = Units.Inches.of(0);
256+
public static final Distance DEADZONE_DISTANCE = Units.Inches.of(0);
252257

258+
public static final AngularVelocity ZEROED_VELOCITY = Units.RotationsPerSecond.of(0.2);
259+
260+
public static final Angle MAX_POS = Units.Degrees.of(57);
261+
public static final Angle MIN_POS = Units.Degrees.of(-37);
262+
263+
public static final Angle ZEROED_MANUAL_POS = Units.Degrees.of(57);
264+
public static final Angle ZEROED_AUTO_POS = Units.Degrees.of(59);
265+
266+
/**
267+
* The elapsed time required to consider the motor as zeroed
268+
*/
269+
public static final Time ZEROED_TIME = Units.Seconds.of(1);
270+
271+
public static final Voltage ZEROING_VOLTAGE = Units.Volts.of(1);
272+
273+
public static TalonFXConfiguration ELEVATOR_LIFT_CONFIG = new TalonFXConfiguration();
274+
public static TalonFXConfiguration ELEVATOR_PIVOT_CONFIG = new TalonFXConfiguration();
275+
276+
static {
277+
// elevator motor config
278+
ELEVATOR_LIFT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
279+
ELEVATOR_LIFT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
280+
ELEVATOR_LIFT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
281+
ELEVATOR_LIFT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Inches.of(62).in(Units.Meters);
282+
ELEVATOR_LIFT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
283+
ELEVATOR_LIFT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Inches.of(0).in(Units.Meters);
284+
ELEVATOR_LIFT_CONFIG.Slot0.GravityType = GravityTypeValue.Elevator_Static;
285+
ELEVATOR_LIFT_CONFIG.Feedback.SensorToMechanismRatio = 0.876;
286+
ELEVATOR_LIFT_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 0;
287+
ELEVATOR_LIFT_CONFIG.MotionMagic.MotionMagicAcceleration = 0;
288+
ELEVATOR_LIFT_CONFIG.MotionMagic.MotionMagicExpo_kV = 0.04;
289+
ELEVATOR_LIFT_CONFIG.MotionMagic.MotionMagicExpo_kA = 0.005;
290+
ELEVATOR_LIFT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
291+
ELEVATOR_LIFT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30;
292+
ELEVATOR_LIFT_CONFIG.CurrentLimits.SupplyCurrentLimit = 60;
293+
ELEVATOR_LIFT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 1;
294+
295+
// elevator pivot motor config
296+
ELEVATOR_PIVOT_CONFIG.MotorOutput.NeutralMode = NeutralModeValue.Brake;
297+
ELEVATOR_PIVOT_CONFIG.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
298+
299+
ELEVATOR_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitEnable = true;
300+
ELEVATOR_PIVOT_CONFIG.SoftwareLimitSwitch.ForwardSoftLimitThreshold = Units.Rotations.of(57)
301+
.in(Units.Degrees);
302+
ELEVATOR_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitEnable = true;
303+
ELEVATOR_PIVOT_CONFIG.SoftwareLimitSwitch.ReverseSoftLimitThreshold = Units.Rotations.of(-37)
304+
.in(Units.Degrees);
305+
306+
ELEVATOR_PIVOT_CONFIG.Feedback.SensorToMechanismRatio = 1000 / 27;// just like intake pivot, we still need
307+
// to get the ratio from fab
308+
309+
ELEVATOR_PIVOT_CONFIG.Slot0.GravityType = GravityTypeValue.Arm_Cosine;
310+
ELEVATOR_PIVOT_CONFIG.Slot0.StaticFeedforwardSign = StaticFeedforwardSignValue.UseClosedLoopSign;
311+
312+
ELEVATOR_PIVOT_CONFIG.MotionMagic.MotionMagicCruiseVelocity = 9999;
313+
ELEVATOR_PIVOT_CONFIG.MotionMagic.MotionMagicAcceleration = 9999;
314+
315+
ELEVATOR_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimitEnable = true;
316+
ELEVATOR_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerLimit = 30;
317+
ELEVATOR_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLimit = 45;
318+
ELEVATOR_PIVOT_CONFIG.CurrentLimits.SupplyCurrentLowerTime = 0.5;
319+
320+
}
253321
}
254322

255323
public static class constClimber {

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,6 @@
1818
import frc.robot.subsystems.StateMachine.RobotState;
1919

2020
import edu.wpi.first.epilogue.Logged;
21-
import edu.wpi.first.epilogue.NotLogged;
2221

2322
@Logged
2423
public class RobotContainer {

‎src/main/java/frc/robot/RobotMap.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -45,10 +45,10 @@ public static class mapIntake {
4545

4646
// Elevator ID 21-30
4747
public static class mapElevator {
48-
public static final int ELEVATOR_LEFT_CAN = 21;
49-
public static final int ELEVATOR_RIGHT_CAN = 22;
50-
public static final int ELEVATOR_RIGHT_PIVOT_CAN = 23;
51-
public static final int ELEVATOR_LEFT_PIVOT_CAN = 24;
48+
public static final int LEFT_LIFT_CAN = 21;
49+
public static final int RIGHT_LIFT_CAN = 22;
50+
public static final int RIGHT_PIVOT_CAN = 23;
51+
public static final int LEFT_PIVOT_CAN = 24;
5252
}
5353

5454
// Climber ID 31-40

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

Lines changed: 80 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -4,27 +4,98 @@
44

55
package frc.robot.subsystems;
66

7+
import static edu.wpi.first.units.Units.Degrees;
8+
import com.ctre.phoenix6.controls.Follower;
9+
import com.ctre.phoenix6.controls.MotionMagicExpoVoltage;
710
import com.ctre.phoenix6.hardware.TalonFX;
8-
11+
import edu.wpi.first.units.Units;
12+
import edu.wpi.first.units.measure.Angle;
13+
import edu.wpi.first.units.measure.AngularVelocity;
14+
import edu.wpi.first.units.measure.Distance;
915
import edu.wpi.first.wpilibj2.command.SubsystemBase;
16+
import frc.robot.Constants;
17+
import frc.robot.Constants.constElevator;
18+
import frc.robot.Robot;
1019
import frc.robot.RobotMap.mapElevator;
1120

1221
public class Elevator extends SubsystemBase {
13-
TalonFX elevatorLeftMotor;
14-
TalonFX elevatorRightMotor;
15-
TalonFX elevatorLeftPivotMotor;
16-
TalonFX elevatorRightPivotMotor;
22+
TalonFX leftLiftMotorFollower;
23+
TalonFX rightLiftMotorLeader;
24+
TalonFX leftPivotMotorFollower;
25+
TalonFX rightPivotMotorLeader;
26+
27+
private Angle lastDesiredAngle = Degrees.zero();
28+
29+
private Distance lastDesiredPosition;
30+
MotionMagicExpoVoltage positionRequest = new MotionMagicExpoVoltage(0);
1731

1832
/** Creates a new Elevator. */
1933
public Elevator() {
2034

21-
elevatorLeftMotor = new TalonFX(mapElevator.ELEVATOR_LEFT_CAN); // Elevator left motor
22-
elevatorRightMotor = new TalonFX(mapElevator.ELEVATOR_RIGHT_CAN); // Elevator right motor
23-
elevatorLeftPivotMotor = new TalonFX(mapElevator.ELEVATOR_LEFT_PIVOT_CAN); // Elevator left pivot motor
24-
elevatorRightPivotMotor = new TalonFX(mapElevator.ELEVATOR_RIGHT_PIVOT_CAN); // Elevator right pivot motor
35+
leftLiftMotorFollower = new TalonFX(mapElevator.LEFT_LIFT_CAN); // Elevator left motor
36+
rightLiftMotorLeader = new TalonFX(mapElevator.RIGHT_LIFT_CAN); // Elevator right motor
37+
leftPivotMotorFollower = new TalonFX(mapElevator.LEFT_PIVOT_CAN); // Elevator left pivot motor
38+
rightPivotMotorLeader = new TalonFX(mapElevator.RIGHT_PIVOT_CAN); // Elevator right pivot motor
2539

40+
lastDesiredPosition = Units.Inches.of(0);
2641
// Set default motor configurations if needed
2742
// e.g., elevatorLeftMotor.configFactoryDefault();
43+
leftLiftMotorFollower.getConfigurator().apply(constElevator.ELEVATOR_LIFT_CONFIG);
44+
rightLiftMotorLeader.getConfigurator().apply(constElevator.ELEVATOR_LIFT_CONFIG);
45+
leftPivotMotorFollower.getConfigurator().apply(constElevator.ELEVATOR_PIVOT_CONFIG);
46+
rightPivotMotorLeader.getConfigurator().apply(constElevator.ELEVATOR_PIVOT_CONFIG);
47+
}
48+
49+
public AngularVelocity getMotorVelocity() {
50+
return rightLiftMotorLeader.getRotorVelocity().getValue();
51+
}
52+
53+
public boolean isMotorVelocityZero() {
54+
return getMotorVelocity().isNear(Units.RotationsPerSecond.zero(), 0.01);
55+
}
56+
57+
public Distance getLastDesiredLiftPosition() {// elevator extension
58+
return lastDesiredPosition;
59+
}
60+
61+
public Distance getLiftPosition() {
62+
if (Robot.isSimulation()) {
63+
return getLastDesiredLiftPosition();
64+
}
65+
return Units.Inches.of(rightLiftMotorLeader.getPosition().getValueAsDouble());
66+
}
67+
68+
public void setLiftPosition(Distance height) {
69+
rightLiftMotorLeader.setControl(positionRequest.withPosition(height.in(Units.Inches)));
70+
leftLiftMotorFollower.setControl(new Follower(rightLiftMotorLeader.getDeviceID(), true));
71+
lastDesiredPosition = height;
72+
}
73+
74+
public boolean isLiftAtSpecificSetpoint(Distance setpoint) {
75+
return isLiftAtSetPointWithTolerance(setpoint, Constants.constElevator.DEADZONE_DISTANCE);
76+
}
77+
78+
public boolean isLiftAtSetPointWithTolerance(Distance position, Distance tolerance) {
79+
if (Robot.isSimulation()) {
80+
return true;
81+
}
82+
return (getLiftPosition()
83+
.compareTo(position.minus(tolerance)) > 0) &&
84+
getLiftPosition().compareTo(position.plus(tolerance)) < 0;
85+
}
86+
87+
public Angle getPivotAngle() {
88+
return rightPivotMotorLeader.getPosition().getValue();
89+
}
90+
91+
public Angle getLastDesiredPivotAngle() {
92+
return lastDesiredAngle;
93+
}
94+
95+
public void setPivotAngle(Angle angle) {
96+
rightPivotMotorLeader.setControl(positionRequest.withPosition(angle.in(Degrees)));
97+
leftPivotMotorFollower.setControl(new Follower(rightPivotMotorLeader.getDeviceID(), true));
98+
lastDesiredAngle = angle;
2899
}
29100

30101
@Override

0 commit comments

Comments
 (0)