Elevator

From the very start of the season, we decided we would use a Cascading elevator to reach different levels of the reef and the barge.

Elevator Specifications

For the second half of this year, our elevator was belt-driven driven by a 2 kraken gearbox with a 5.7-1 ratio. This allowed us to aggressively drive it both up and down, opposed to only being able to drive it agressively up earlier in the season because of a previously string-driven elevator.

Subsystem Code

First off: We will see the following hardware in the elevator subsystem:

2 krakens (leader and follower) that drive the elevator
1 DIO Limit Switch that resets the position of the elevator

Constructor

public class Lift extends SubsystemABS {
    private TalonFX elevatorMotorLeader; // Primary motor
    private TalonFX elevatorMotorFollower; // Follower motor
    private DoubleSupplier m_encoderValue;
    public DoubleSupplier m_elevatorSpeed;
    private BooleanSupplier elevatorAboveThreshold;
    private DigitalInput limitSwitch;
 
    // private final ShuffleboardTab tab = Shuffleboard.getTab("Elevator");
    private final PIDController pid;
    private final PIDController pidDown;
 
    public Lift(Subsystems subsystem, String name) {
        super(subsystem, name);
        elevatorMotorLeader = new TalonFX(RobotMap.ElevatorMap.ELEVATOR_MOTOR);
        elevatorMotorFollower = new TalonFX(RobotMap.ElevatorMap.ELEVATOR_MOTOR2);
        limitSwitch = new DigitalInput(9);
       
        // Configure follower motor
        elevatorMotorFollower.setControl(new Follower(elevatorMotorLeader.getDeviceID(), false));
 
        // Configure current limits
        elevatorMotorLeader.getConfigurator().apply(
                RobotMap.CurrentLimiter.getCurrentLimitConfiguration(RobotMap.ElevatorMap.ELEVATOR_CURRENT_LIMIT));
        elevatorMotorFollower.getConfigurator().apply(
                RobotMap.CurrentLimiter.getCurrentLimitConfiguration(RobotMap.ElevatorMap.ELEVATOR_CURRENT_LIMIT));
 
        m_encoderValue = () -> elevatorMotorLeader.getPosition().getValueAsDouble();
        
        elevatorAboveThreshold = ()-> getElevatorAboveThreshold();
 
        pid = new PIDController(RobotMap.ElevatorMap.ELEVATOR_P, RobotMap.ElevatorMap.ELEVATOR_I, RobotMap.ElevatorMap.ELEVATOR_D);
        pid.setTolerance(.2);
 
        pidDown = new PIDController(0.4, 0, 0);
        pidDown.setTolerance(.2);
        pidDown.setSetpoint(1.3);
 
        tab.add("Elevator PID", pid)
            .withWidget(BuiltInWidgets.kPIDController);
 
        tab.add("Elevator PID Down", pidDown)
            .withWidget(BuiltInWidgets.kPIDController);
 
        tab.addNumber("Elevator Position", m_encoderValue);
 
        GenericEntry elevatorSpeedSetter = tab.add("Elevator Speed", 0.0)
                .withWidget(BuiltInWidgets.kNumberSlider)
                .withProperties(Map.of("min", 0, "max", .2))
                .getEntry();
        m_elevatorSpeed = () -> elevatorSpeedSetter.getDouble(0);
 
        tab.addBoolean("elevator Threshold" , elevatorAboveThreshold);
    }
}

Constructor Explanation

Private Member Fields

Field	What it is	Why it’s here
`TalonFX elevatorMotorLeader`	Kraken X60 that actually drives the elevator	Primary motor you control.
`TalonFX elevatorMotorFollower`	Second Kraken X60	Electrically follows the leader for more torque.
`DoubleSupplier m_encoderValue`	Lambda returning the leader’s position	Handy for Shuffleboard & PID.
`DoubleSupplier m_elevatorSpeed`	Lambda that reads a Shuffleboard slider	Lets you live-tune manual speed from the DS.
`BooleanSupplier elevatorAboveThreshold`	Lambda that checks if lift is past a certain height	Useful for logic such as “only shoot when up.”
`DigitalInput limitSwitch`	Hard-wired top/bottom limit switch on DIO 9	Zeroing or hard-stop safety.
`PIDController pid`	PID used when moving up	Constants pulled from `RobotMap`.
`PIDController pidDown`	PID used when moving down	Different P-gain and a fixed set-point of 1.3 rev (counteracts gravity).

Motor and limit switch instantiation

First, we instantiate our motors and limit switch:

    elevatorMotorLeader = new TalonFX(RobotMap.ElevatorMap.ELEVATOR_MOTOR);
    elevatorMotorFollower = new TalonFX(RobotMap.ElevatorMap.ELEVATOR_MOTOR2);
    limitSwitch = new DigitalInput(9);

Motor Configuration

 // Configure follower motor
        elevatorMotorFollower.setControl(new Follower(elevatorMotorLeader.getDeviceID(), false));
 
        // Configure current limits
        elevatorMotorLeader.getConfigurator().apply(
                RobotMap.CurrentLimiter.getCurrentLimitConfiguration(RobotMap.ElevatorMap.ELEVATOR_CURRENT_LIMIT));
        elevatorMotorFollower.getConfigurator().apply(
                RobotMap.CurrentLimiter.getCurrentLimitConfiguration(RobotMap.ElevatorMap.ELEVATOR_CURRENT_LIMIT));

We use .setControl() to make our follower motor follow the output of the leader; false means it rotates in the same direction.
Then, we apply a current limiter configuration using a CurrentLimiter object found in the RobotMap

The RobotMap class contains all of the constants for our robot. Because our current limit is a constant configuration, we keep it there for the sake of organization. The constant for the elevator current limit was 50 amps.

PID Controllers

    pid = new PIDController(RobotMap.ElevatorMap.ELEVATOR_P, RobotMap.ElevatorMap.ELEVATOR_I, RobotMap.ElevatorMap.ELEVATOR_D);
    pid.setTolerance(.2);
 
    pidDown = new PIDController(0.4, 0, 0);
    pidDown.setTolerance(.2);
    pidDown.setSetpoint(1.3);

We had 2 PID Controllers for our elevator this year. the pid PID gains are .53, 0, and 0.

We had a slightly less agressive PID controller for down movement to prevent occasional loop overrun related issues. These can be prevented by using the onboard 1Khz PID control (opens in a new tab) on Kraken x60's, which we will be using in the future for more consistent control.

Shuffleboard Integrations

Using Shuffleboard, we integrated useful readouts and useful debugging features with our elevator:

tab.add("Elevator PID", pid).withWidget(kPIDController);
tab.add("Elevator PID Down", pidDown).withWidget(kPIDController);
tab.addNumber("Elevator Position", m_encoderValue);

Live tunable PID Widgets allowing for real-time updating and a readout for elevator encoder value.

GenericEntry elevatorSpeedSetter = tab.add("Elevator Speed", 0.0)
        .withWidget(kNumberSlider)
        .withProperties(Map.of("min", 0, "max", .2))
        .getEntry();
m_elevatorSpeed = () -> elevatorSpeedSetter.getDouble(0);

This creates a slider (limited to 0 to 0.2) that feeds a lambda so elevator commands can read the chosen manual speed, used to find elevator constants.

tab.addBoolean("elevator Threshold" , elevatorAboveThreshold);

This makes a red/green light indicator that tells robot operators whether or not the elevator is above the threshold that activates a swerve "Slow mode", to prevent tipping.

Additional Missing Pieces

This constructor contains a few methods such as getElevatorAboveThreshold() that will be explained later in the elevator code dive.

`Periodic() Method`

 public void periodic() {
        m_encoderValue = () -> elevatorMotorLeader.getPosition().getValueAsDouble();
        elevatorAboveThreshold = ()-> getElevatorAboveThreshold();
 
        SmartDashboard.putBoolean("limit switch", elevatorSwitchTriggered());
    }

The Periodic method runs in a loop, which means it's useful for updating values for shuffleboard.

The m_encoderValue and elevatorAboveThreshold variables are both updated continuously for dashboard display
The output of the elevatorSwitchTriggered() method is displayed to the dashboard, for debugging and system checks.

Important Methods

    public void setMotorVolt(double volt){
        elevatorMotorLeader.setControl(new VoltageOut(volt));
    }
 
    public void setPIDTarget(double target) {
        pid.setSetpoint(target);
    }
 
    public boolean pidAtSetpoint() {
        return pid.atSetpoint();
    }
 
    public boolean elevatorSwitchTriggered(){
        return !limitSwitch.get();
    }
 
    public void rotateElevatorPID() {
        double output = pid.calculate(getEncoderValue());
        if(output > 0){
            output += ElevatorMap.ELEVATOR_S;
        } else {
            output -= ElevatorMap.ELEVATOR_S;
        }
        output += ElevatorMap.ELEVATOR_G;
        setMotorSpeed(output);
    }
 
    public void rotateElevatorPIDDown(){
        double output = pidDown.calculate(getEncoderValue());
        if(output > 0){
            output += ElevatorMap.ELEVATOR_S;
        } else {
            output -= ElevatorMap.ELEVATOR_S;
        }
        output += ElevatorMap.ELEVATOR_G;
        setMotorSpeed(output);
    }
 
    public boolean pidDownAtSetpoint(){
        return pidDown.atSetpoint();
    }
 
    public double getEncoderValue() {
        return m_encoderValue.getAsDouble();
    }
 
    public double getElevatorHeight() {
        return m_encoderValue.getAsDouble() * ElevatorMap.ELEVATOR_CIRCUMFERENCE;
    }
 
    public void setElevatorAtLimitHeight(){
        elevatorMotorLeader.setPosition(.45);
    }
 
    public boolean getElevatorAboveThreshold(){
        return getEncoderValue() > RobotMap.ElevatorMap.L3ROTATION + 1;
    }
 
    public boolean getElevatorAtBelowL4(){
        return getEncoderValue() < RobotMap.ElevatorMap.L4ROTATION;
    }
 
    public boolean getElevatorAtBarge(){
        return getEncoderValue() > RobotMap.ElevatorMap.BARGEROTATION-1;
    }

1. `setMotorVolt(double volt)`

Directly commands the leader Kraken to output a specific voltage using CTRE’s VoltageOut control mode. We used this as opposed to .set() to prevent diminishing and inconsistent motor output after battery use during a match.

2. `setPIDTarget(double target)`

Updates the setpoint of the main (upward) PID controller so the elevator will try to reach the given encoder position.

3. `pidAtSetpoint()`

Returns true when the main PID reports it is within its tolerance band of the target; otherwise false.

4. `elevatorSwitchTriggered()`

Returns true when the physical limit switch is pressed. The boolean is inverted (!) because the switch is wired “normally closed.”

5. `rotateElevatorPID()`

Runs the elevator upward under PID control:

Calculates PID output from the current encoder value.
Adds static friction compensation (ELEVATOR_S, sign-dependent).
Adds gravity compensation (ELEVATOR_G).
Sends the final speed command to the motor.

6. `rotateElevatorPIDDown()`

Same idea as method 5 but uses the separate downward PID (pidDown).

7. `pidDownAtSetpoint()`

Returns true when the downward PID controller is within tolerance of its fixed setpoint (default = 1.3 rot).

8. `getEncoderValue()`

A shorthand accessor that returns the raw encoder rotations as a double.

9. `getElevatorHeight()`

Converts the encoder rotations to a linear height by multiplying by the elevator drum’s circumference.

10. `setElevatorAtLimitHeight()`

Manually resets (or “zeros”) the encoder position to 0.45 rotations. We set the elevator encoder to .45 instead of 0 because the limit switch triggers slightly above the "true zero" position.

11. `getElevatorAboveThreshold()`

Checks whether the elevator is above the Level-3 rotation threshold plus a 1-rotation safety buffer.

12. `getElevatorAtBelowL4()`

Checks whether the elevator is below the Level-4 rotation value.

13. `getElevatorAtBarge()`

Checks whether the elevator is above the “barge” preset height (with a 1-rotation negative buffer).

Conclusion

And that's the Elevator subsystem code! Check it out in full on our github page (opens in a new tab).

Subsystem Commands

Elevator

Elevator Specifications

Subsystem Code

Constructor

Constructor Explanation

Private Member Fields

Motor and limit switch instantiation

Motor Configuration

PID Controllers

Shuffleboard Integrations

Additional Missing Pieces

Periodic() Method

Important Methods

1. setMotorVolt(double volt)

2. setPIDTarget(double target)

3. pidAtSetpoint()

4. elevatorSwitchTriggered()

5. rotateElevatorPID()

6. rotateElevatorPIDDown()

7. pidDownAtSetpoint()

8. getEncoderValue()

9. getElevatorHeight()

10. setElevatorAtLimitHeight()

11. getElevatorAboveThreshold()

12. getElevatorAtBelowL4()

13. getElevatorAtBarge()