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();
}
}