Help with PID Arm

Technical Java
1

We are trying to use a PID controller for our 1 DOF arm, and I can’t get the PID to work. I have a FeedForward that works when the PID controller is commented out, but with the PID controller in the Talon tells the arm to reverse at full speed. Can anyone help with this?

package frc.robot.subsystems;

import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.DutyCycleEncoder;
import edu.wpi.first.wpilibj.PowerDistribution;
import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.RobotContainer;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.can.TalonSRXConfiguration;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;

public class Arm extends SubsystemBase {
  /** Creates a new Arm. */
  public static WPI_TalonSRX armMaster;
  public static WPI_TalonSRX armFollower;

  static double kDt = 0.02;

  public static DutyCycleEncoder armEncoder = new DutyCycleEncoder(0); 
  

  static double maxGravityFF = 0.1;
  static double kMeasuredPosHorizontal = 11650; //Position measured when arm is horizontal
  static double kTicksPerDegree = 8192 / 360; //Sensor is 1:1 with arm rotation
  static double currentPos = armEncoder.getDistance();
  

public static  double Scalar() {
  armEncoder.setDistancePerRotation(8192);
  currentPos = armEncoder.getDistance();
  double degrees = (currentPos - kMeasuredPosHorizontal) / kTicksPerDegree;
  double radians = java.lang.Math.toRadians(degrees);
  double cosineScalar = java.lang.Math.cos(radians);
  
  return cosineScalar;
}


  public static TalonSRXConfiguration config2 = new TalonSRXConfiguration();

  static double position0 = 1.4;
  static double position1 = .1;
  static double position2 = -.1;
  static double position3 = .3;
  static double position4 = .4;
  static double position5 = .5;

  //PID Constants

static double Kp = 1;
static double Ki = 0;
static double Kd = 0;

static PowerDistribution pdp = new PowerDistribution(0, ModuleType.kCTRE);
   
  static TrapezoidProfile.Constraints m_constraints =
  new TrapezoidProfile.Constraints(10, 15);
public static ProfiledPIDController m_controller =
  new ProfiledPIDController(Kp, Ki, Kd, m_constraints, kDt);

  public static void Distance() {
    armEncoder.setDistancePerRotation(1);
    currentPos = armEncoder.getDistance();
   
    //throughBore.setDistancePerRotation(1.0 / 360.0 * 2.0 * Math.PI * .028);

    SmartDashboard.putNumber("Arm Encoder", currentPos);
    //SmartDashboard.putNumber("Arm Degrees", degrees); 
  }
  
  public Arm() {
    armMaster = new WPI_TalonSRX(1);
  }

  public static void MoveArm() {
    armEncoder.setDistancePerRotation(1);
    if (RobotContainer.operator.getAButton() == true) {
     armMaster.set(ControlMode.MotionMagic, position0, DemandType.ArbitraryFeedForward, -maxGravityFF * Scalar());
   m_controller.setGoal(position0);
   armMaster.set(m_controller.calculate(Arm.armEncoder.getDistance()));
    }
   /* else if (RobotContainer.operator.getBButtonPressed() == true) {
      armMaster.set(ControlMode.MotionMagic, position1, DemandType.ArbitraryFeedForward, maxGravityFF * Scalar());
      m_controller.setGoal(position1);
    }
    else if (RobotContainer.operator.getXButtonPressed() == true) {
      armMaster.set(ControlMode.MotionMagic, position2, DemandType.ArbitraryFeedForward, maxGravityFF * Scalar());
      m_controller.setGoal(position2);
    }
  public static void Voltages() {
    double current4 = pdp.getCurrent(4);
    SmartDashboard.putNumber("Manipulator", current4);
  

    double current3 = pdp.getCurrent(3);
    SmartDashboard.putNumber("Arm", current3);

   

   
  }
    

  @Override
  public void periodic() {
    // This method will be called once per scheduler run
    CommandScheduler.getInstance().setDefaultCommand(RobotContainer.m_arm, RobotContainer.m_moveArm);
  }
}
2

Just from a semi-quick look at your code:

In the MoveArm() method, your getAButton() if case sets the arm motor to motion magic control, and then immediately sets it using the simple set() method based on the PID controller’s output. Motion magic relys on the internal PID controller the Talon runs. Its gains need to be set through the CTRE api. You create a TalonSRXConfiguration, but don’t do anything with it nor apply it to the Talon.

Also, calling the set() method immediately after overrides the initial call, so the motor will use whatever the PID controller outputs as its output percentage instead of the motion magic.

3

Since the second set method overrides the first, how would I use the feedforward and tell the motor to go to a certain position using Motion Magic/PID at the same time?

4

motion magic already uses PID to set the motor output.
You use the CTRE api to set the PID constants (and a static feed forward if you’d like) and then call set() with a MotionMagic control mode and an arbitrary feed forward of whatever you calculated (I believe arbitrary feed forwards need to be in native units).

CTRE has a bunch of helpful info on their docs for setting up / trying motion magic:

1 Like