Elevator control example

The elevator on the robot is controlled using motion-profiled PID controllers. The code is described below to aid in creating similar components (position-controlled arm, etc.).

// Sets the package the code is in, this should generally match the folder path - "/src/main/java/com/pigmice/frc/robot/subsystems"
package com.pigmice.frc.robot.subsystems;

// Imports other packages to use in this code
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.pigmice.frc.lib.motion.Setpoint;
import com.pigmice.frc.lib.motion.StaticProfile;
import com.pigmice.frc.lib.motion.execution.StaticProfileExecutor;
import com.pigmice.frc.lib.utils.Range;
import com.pigmice.frc.lib.utils.Utils;

// Creates a component named "Elevator"
public class Elevator {
    // tells Java the Elevator component needs to store a reference to a TalonSRX motor controller
    // private says the motor controller should only be accessible from inside this component
    // "winch" is the name used to reference the motor controller throughout the component.
    // By default, the motor controller will be equal to "null" - it has no value yet.
    private TalonSRX winch;

    // private variable named targetPosition, Double is the type used for decimal numbers
    // This stores the position we want the elevator to be at
    private Double targetPosition;
    // Stores the position the elevator is currently at.
    private Double currentPosition;

    // Range is a custom type the stores a range of numbers, in this case it stores the minimum and maximum values the
    // elevator sensor will read, at the bottom and top of its range of motion respectively.
    private Range sensorBounds = new Range(0, 30400.0);

    // StaticProfileExecutor is the type holding a motion-profile controller, which plans the elevator motion to make it 
    // start and stop smoothly.
    private StaticProfileExecutor profileExecutor;

    // Feedforward coefficient - this is not necessary for a simple controller, but helps boost motor output when the
    // controller wants the elevator to be moving quickly 
    private double kF = 0.85;
    // Tuning coefficient to compensate for the fact that gravity makes the elevator move faster down than up
    private double gravityCompensation = 0.055;

    // Java constructor function - takes a motor as input, and stores it into the "winch" variable.
    public Elevator(TalonSRX winchMotor) {
        winch = winchMotor;
        // Configures the coefficients for the PID controller built into TalonSRX motor controller.
        winchMotor.config_kP(0, 0.2, 10);
        winchMotor.config_kI(0, 0.0, 10);
        winchMotor.config_kD(0, 0.0, 10);
        winchMotor.config_kF(0, 0.0, 10);
        winchMotor.configNeutralDeadband(0.005, 10);

        // When this component is created, the elevator is assumed to resting on the "kickstand" bar, so the sensor is 
        // reset to 0.138, since the elevator is 13.8% of the way to the top in that position
        zeroSensor();
        // Sets a target of 13.8% up to it doesn't move right away.
        setTargetPosition(0.138);
    }

    // Function that takes a percent of full power, and applies it to the winch motor.
    public void drive(double percent) {
        winch.set(ControlMode.PercentOutput, percent);
    }

    // Takes a number from 0-1, 0 being the bottom and 1 being the top, for the elevator to target.
    public void setTargetPosition(double targetPosition) {
        // If the profileExecutor variable is set to "null" - empty - or the target position input is different than the
        // current target position by more than a rounding error, run the rest of this code.
        // This prevents a bunch of code running even when the target position set is the same as the current target.
        if (profileExecutor == null || Math.abs(this.targetPosition - targetPosition) > 1e-2) {
            // PID controllers remember how much friction they were last compensating for, etc. Since we might be
            // drastically changing what the elevator is attempting to do, reset the PID in the motor controller.
            resetPID();
            // Creates a temporary variable to store a StaticProfile - a custom component that calculates the exact plan
            // to move the elevator from its current position to the target position
            StaticProfile profile;
           
            // Given current speed, position, target, and some parameters representing how fast the elevator can
            // accelerate, move, and decelerate, plan out the motion it will use to get to the target.
            profile = new StaticProfile(getVelocity(), currentPosition, targetPosition, 2.0, 1.6, 1.6);
            
            // Store the input target position into the targetPosition variable that is part of the elevator component
            // so it stays around after this function finishes running
            this.targetPosition = targetPosition;
            // a StaticProfileExecutor follows the StaticProfile's calculated motion plan to move the elevator.
            profileExecutor = new StaticProfileExecutor(profile, this::output, this::getPosition, 0.02);
            profileExecutor.initialize();
        }
    }

    public double getPosition() {
        // Raw sensor value in encoder ticks.
        double raw = (double) winch.getSelectedSensorPosition();
        // Linear interpolation - minimum sensor value is mapped to 0, maximum to 1, everything else falls in-between.
        return Utils.lerp(raw, sensorBounds.min(), sensorBounds.max(), 0.0, 1.0);
    }

    public double getVelocity() {
        double raw = (double) winch.getSelectedSensorVelocity();
        // Maps from sensor ticks per 100nms to values that make more sense with the position units used. 
        return Utils.lerp(raw, -sensorBounds.size(), sensorBounds.size(), -1.0, 1.0);
    }

    public void zeroSensor() {
        currentPosition = 0.138;
        // Maps from 0.138 back into native sensor ticks and sets the motor sensor to that value.
        winch.setSelectedSensorPosition((int) Utils.lerp(0.138, 0, 1.0, sensorBounds.min(), sensorBounds.max()));
    }

    // Reads the converted sensor value and stores it into this component. This means the sensor can be read just once
    // and that value can be reused throughout the component without re-reading and re-converting it many times.
    public void updateSensor() {
        currentPosition = getPosition();
    }
    
    // Tells the StaticProfileExecutor to run an update cycle
    public void update() {
        profileExecutor.updateNoEnd();
    }

    public void resetPID() {
        winch.setIntegralAccumulator(0.0, 0, 10);
    }

    // This function is passed into the StaticProfileExecutor. Given an input value of the custom type "Setpoint" which
    // contains the speed the elevator should be going, the position it should be at, etc. it gives those values to the
    // PID controller in the motor which adjusts the motor output to keep the elevator on track for the planned motion.
    private void output(Setpoint sp) {
        double lerp = Utils.lerp(sp.getPosition(), 0.0, 1.0, sensorBounds.min(), sensorBounds.max());
        winch.set(ControlMode.Position, lerp, DemandType.ArbitraryFeedForward,
                gravityCompensation + kF * sp.getVelocity());
    }
}