Help with controlling our TalonFX by position

Technical Java
1

We are trying to use a PID controller for our frc elevator using the position control mode of the TalonFX, I’m attaching our code below. Anyone knows how to get it done?

package frc.robot.subsystems;

import edu.wpi.first.wpilibj2.command.SubsystemBase;

import com.ctre.phoenix.motorcontrol.can.TalonFX;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;

import frc.robot.Constants;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkMaxLowLevel.MotorType;


public class Brazo extends SubsystemBase {
  /** Creates a new Brazo. */
  private final TalonFX CamMotor = new TalonFX(Constants.CamMotor_ID);

  private final TalonFX RailMotor = new TalonFX(Constants.CamMotor_ID);
  //private final CANSparkMax RailMotor = new CANSparkMax(Constants.RailMotor_ID, MotorType.kBrushless);

  public Brazo() {}

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
  }


  public void Retracted(){
    CamMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    CamMotor.configNominalOutputForward(0, 0);
    CamMotor.setInverted(false);
    CamMotor.setSensorPhase(false);
    CamMotor.config_kP(0, 0, 0);
    CamMotor.config_kI(0, 0, 0);
    CamMotor.config_kD(0, 0, 0);
    CamMotor.set(ControlMode.Position, 1);

    RailMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    RailMotor.configNominalOutputForward(0, 0);
    RailMotor.setInverted(false);
    RailMotor.setSensorPhase(false);
    RailMotor.config_kP(0, 0, 0);
    RailMotor.config_kI(0, 0, 0);
    RailMotor.config_kD(0, 0, 0);
    RailMotor.set(ControlMode.Position, 1);
    //RailMotor.getPIDController();

  }

  public void Lowlevel(){
    CamMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    CamMotor.configNominalOutputForward(0, 0);
    CamMotor.setInverted(false);
    CamMotor.setSensorPhase(false);
    CamMotor.config_kP(0, 0, 0);
    CamMotor.config_kI(0, 0, 0);
    CamMotor.config_kD(0, 0, 0);
    CamMotor.set(ControlMode.Position, 1);

    RailMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    RailMotor.configNominalOutputForward(0, 0);
    RailMotor.setInverted(false);
    RailMotor.setSensorPhase(false);
    RailMotor.config_kP(0, 0, 0);
    RailMotor.config_kI(0, 0, 0);
    RailMotor.config_kD(0, 0, 0);
    RailMotor.set(ControlMode.Position, 1);
    //RailMotor.getPIDController();

  }

  public void Middlelevel(){
    CamMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    CamMotor.configNominalOutputForward(0, 0);
    CamMotor.setInverted(false);
    CamMotor.setSensorPhase(false);
    CamMotor.config_kP(0, 0, 0);
    CamMotor.config_kI(0, 0, 0);
    CamMotor.config_kD(0, 0, 0);
    CamMotor.set(ControlMode.Position, 0);

    RailMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    RailMotor.configNominalOutputForward(0, 0);
    RailMotor.setInverted(false);
    RailMotor.setSensorPhase(false);
    RailMotor.config_kP(0, 0, 0);
    RailMotor.config_kI(0, 0, 0);
    RailMotor.config_kD(0, 0, 0);
    RailMotor.set(ControlMode.Position, 0);
    //RailMotor.getPIDController();
  }

  public void Highlevel(){
    CamMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    CamMotor.configNominalOutputForward(0, 0);
    CamMotor.setInverted(false);
    CamMotor.setSensorPhase(false);
    CamMotor.config_kP(0, 0, 0);
    CamMotor.config_kI(0, 0, 0);
    CamMotor.config_kD(0, 0, 0);
    CamMotor.set(ControlMode.Position, 1);

    RailMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 0);
    RailMotor.configNominalOutputForward(0, 0);
    RailMotor.setInverted(false);
    RailMotor.setSensorPhase(false);
    RailMotor.config_kP(0, 0, 0);
    RailMotor.config_kI(0, 0, 0);
    RailMotor.config_kD(0, 0, 0);
    RailMotor.set(ControlMode.Position, 1);
    //RailMotor.getPIDController();
  }
}
2

You only need to configure your onboard motor controllers PID gains once. You should probably do that along with all your other motor configuration code in your constructor. With elevators, most of the time a P controller is good enough. What I would do is test this code first on your drivetrain motors so you don’t accidentally break your elevator. The way the PID gains are used in the algorithm is they are the coefficients in the three equations but since you only need to use the proportional equation you could set I and D to 0. Tuning is all dependent on your gearing but you could start with like 0.1.

When you do

motor.set(ControlMode.Position, position)

that position is in terms of encoder counts/ticks. You need a way to zero out your encoders. One way is to manually move them down with phoenix tuner while the robot is enabled in test mode as test mode should disable your commands and subsystems or another way would be to mechanically move it down and then power cycle the robot and just reset your encoders on startup to 0.

You can get the current encoder position by taking a self test snapshot in phoenix tuner or by simply displaying the data on SmartDashboard with motor.getSelectedSensorPosition(). For how to actually implement the different “levels” you could look at how we did it in our code.