TalonFX always goes to max velocity

Monarch49 · February 9, 2022, 8:47pm

Hi there! We are currently working on fixing a problem with the phoenix tuner. Essentially, using velocity closed-loop, our motor always goes to the max velocity unless it is set to 0 when it stops (if it’s set to negative it just goes to max velocity in the negative direction). We can’t understand why, this happens when setting velocity using the tuner and when setting velocity in code. Percent output works fine though. Any help is appreciated!
-Team 5000

dan · February 9, 2022, 9:03pm

Can you send the code?

Monarch49 · February 9, 2022, 9:08pm

Here’s the code:
‘’’
package frc.robot.subsystems;

import com.ctre.phoenix.motorcontrol.ControlMode;

import com.ctre.phoenix.motorcontrol.FeedbackDevice;

import com.ctre.phoenix.motorcontrol.NeutralMode;

import com.ctre.phoenix.motorcontrol.TalonFXControlMode;

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

import edu.wpi.first.math.controller.SimpleMotorFeedforward;

import edu.wpi.first.networktables.NetworkTable;

import edu.wpi.first.networktables.NetworkTableEntry;

import edu.wpi.first.networktables.NetworkTableInstance;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

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

import frc.robot.Constants;

public class ShooterSubsystem extends SubsystemBase {

private static TalonFX leftShooterMotor = new TalonFX(Constants.SHOOTER_LEFT_MOTOR_PORT);

private static TalonFX rightShooterMotor = new TalonFX(Constants.SHOOTER_RIGHT_MOTOR_PORT);

//(x units/100ms) = (rpm * Constants.K_SENSOR_UNITS_PER_ROTATION/600(units/100ms))

double testF;

double testP;

double testI;

double testD;

double testError;

double rpm = 5600;

//limelight isn’t currently doing anything

NetworkTable table = NetworkTableInstance.getDefault().getTable(“limelight”);

NetworkTableEntry ty = table.getEntry(“ty”);

/** Creates a new ShooterSubsystem. */

public ShooterSubsystem() {

leftShooterMotor.setNeutralMode(NeutralMode.Coast);

rightShooterMotor.setNeutralMode(NeutralMode.Coast);

//TODO: Tune PID values

//TODO: Create RPM Aim class to calculate needed rpm as separate aim button, then create object here to use a getter in the feedForward.calculate

//So essentially it appears we will be putting the limelight part in the constructor? I'm not entirely sure how this is

//going to work but we'll figure it out.



SimpleMotorFeedforward feedForward = new SimpleMotorFeedforward(Constants.kS, Constants.kV, Constants.kA);

testF = feedForward.calculate(rpm);

testP = Constants.kGains.kP;

testI = Constants.kGains.kI;

testD = Constants.kGains.kD;

testError = 0;



//left motor specific stuff

leftShooterMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, Constants.PID_LOOP_IDX, Constants.TIMEOUT_MS);

leftShooterMotor.setSensorPhase(Constants.PHASE_SENSOR);

leftShooterMotor.configNominalOutputForward(0, Constants.TIMEOUT_MS);

leftShooterMotor.configNominalOutputReverse(0, Constants.TIMEOUT_MS);

leftShooterMotor.configPeakOutputForward(Constants.kGains.kPeakOutput, Constants.TIMEOUT_MS);

leftShooterMotor.configPeakOutputReverse(-Constants.kGains.kPeakOutput, Constants.TIMEOUT_MS);



leftShooterMotor.configAllowableClosedloopError(Constants.PID_LOOP_IDX, (Constants.K_SENSOR_UNITS_PER_ROTATION / 600.0) * testError, Constants.TIMEOUT_MS);

leftShooterMotor.config_kF(Constants.PID_LOOP_IDX, testF, Constants.TIMEOUT_MS);  

leftShooterMotor.config_kP(Constants.PID_LOOP_IDX, testP, Constants.TIMEOUT_MS);

leftShooterMotor.config_kI(Constants.PID_LOOP_IDX, testI, Constants.TIMEOUT_MS);

leftShooterMotor.config_kD(Constants.PID_LOOP_IDX, testD, Constants.TIMEOUT_MS);

//right motor specific stuff



rightShooterMotor.configSelectedFeedbackSensor(FeedbackDevice.IntegratedSensor, Constants.PID_LOOP_IDX, Constants.TIMEOUT_MS);

rightShooterMotor.setSensorPhase(Constants.PHASE_SENSOR);

rightShooterMotor.configNominalOutputForward(0, Constants.TIMEOUT_MS);

rightShooterMotor.configNominalOutputReverse(0, Constants.TIMEOUT_MS);

rightShooterMotor.configPeakOutputForward(Constants.kGains.kPeakOutput, Constants.TIMEOUT_MS);

rightShooterMotor.configPeakOutputReverse(-Constants.kGains.kPeakOutput, Constants.TIMEOUT_MS);

rightShooterMotor.configAllowableClosedloopError(Constants.PID_LOOP_IDX, (Constants.K_SENSOR_UNITS_PER_ROTATION / 600.0) * testError, Constants.TIMEOUT_MS);

rightShooterMotor.config_kF(Constants.PID_LOOP_IDX, testF, Constants.TIMEOUT_MS);  

rightShooterMotor.config_kP(Constants.PID_LOOP_IDX, testP, Constants.TIMEOUT_MS);

rightShooterMotor.config_kI(Constants.PID_LOOP_IDX, testI, Constants.TIMEOUT_MS);

rightShooterMotor.config_kD(Constants.PID_LOOP_IDX, testD, Constants.TIMEOUT_MS);

}

@Override

public void periodic() {

// This method will be called once per scheduler run

}

public void m_TurnOnLimelight(){

table.getEntry("ledMode").setNumber(0);

}

public void m_TurnOffLimelight(){

table.getEntry("ledMode").setNumber(1);

}

public void m_shoot()

{

double y = ty.getDouble(0.0);

double heading_error = -y;



SmartDashboard.putNumber("rpm", rpm);

double motorSpeed = (Constants.K_SENSOR_UNITS_PER_ROTATION / 600.0) * rpm;

//600 is a modifer to get min to 100 ms and 2048 gets rotations to units

//Right now I'm putting the motors at desired rpm for testing purposes 6380 or whatever number is after (2048 / 600) will change to rpm

leftShooterMotor.set(TalonFXControlMode.Velocity, motorSpeed);

rightShooterMotor.set(TalonFXControlMode.Velocity, -motorSpeed);

//leftShooterMotor.set(TalonFXControlMode.PercentOutput, 0.65);

//rightShooterMotor.set(TalonFXControlMode.PercentOutput, -0.65);

SmartDashboard.putNumber(“F”, testF);

SmartDashboard.putNumber(“P”, testP);

SmartDashboard.putNumber(“I”, testI);

SmartDashboard.putNumber(“D”, testD);

SmartDashboard.putNumber(“Error”, testError);

SmartDashboard.putNumber(“RPM”, ( (600.0 / Constants.K_SENSOR_UNITS_PER_ROTATION) * leftShooterMotor.getSelectedSensorVelocity()));

String motorState;

if(leftShooterMotor.getSelectedSensorVelocity(Constants.PID_LOOP_IDX) > 0){

 motorState = "forward";

}else if(leftShooterMotor.getSelectedSensorVelocity(Constants.PID_LOOP_IDX) == 0){

 motorState = "stopped";

}else{

 motorState = "reverse";

}

SmartDashboard.putString(“Motor State”, motorState);

}

public void m_stopSpinning()

{

leftShooterMotor.set(TalonFXControlMode.PercentOutput, 0.0);//it's a stray number but you can tell it makes the motor spin at 0%

rightShooterMotor.set(TalonFXControlMode.PercentOutput, 0.0);

}

public void m_zeroEncoder(){

leftShooterMotor.setSelectedSensorPosition(0);

rightShooterMotor.setSelectedSensorPosition(0);

}

public double m_getPosition(){

return leftShooterMotor.getSelectedSensorPosition();

}

}
‘’’

Sidenote: the reason there’s pid values assigned here is because we are still testing and it’s easier to change them in the same class. Also the rpm is just a set variable now because we don’t have our limelight ready to go.

dan · February 9, 2022, 9:20pm

Could you please upload the code to GitHub/Pastebin, or at least edit your post and wrap the code with three ticks (`) on each side? I’m on my phone and it’s pretty difficult to read the code that way.

Monarch49 · February 9, 2022, 9:24pm

It’s under the morePIDWorkOnFlywheels branch. LMK if you can’t access it and I can shoot my mentors an email.

dan · February 9, 2022, 9:41pm

From a quick glance, it seems that you set the RPM to a constant value and never change it:

github.com

hammerheads5000/2022RapidReact/blob/35b727016c1b0ca390f726dcc6ac8254b5a096c6/src/main/java/frc/robot/subsystems/ShooterSubsystem.java#L38


//(x units/100ms) = (rpm * Constants.K_SENSOR_UNITS_PER_ROTATION/600(units/100ms))


double testF;
double testP;
double testI;
double testD;
double testError;


double rpm = 5600;


//limelight isn't currently doing anything
 NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
 NetworkTableEntry ty = table.getEntry("ty");

  /** Creates a new ShooterSubsystem. */
  public ShooterSubsystem() {
    leftShooterMotor.setNeutralMode(NeutralMode.Coast);
    rightShooterMotor.setNeutralMode(NeutralMode.Coast);

Then you refer to it where you set the motors’ velocities:

github.com

hammerheads5000/2022RapidReact/blob/morePidWorkOnFlywheels/src/main/java/frc/robot/subsystems/ShooterSubsystem.java#L120-L124


 double motorSpeed = (Constants.K_SENSOR_UNITS_PER_ROTATION / 600.0) * rpm;
 //600 is a modifer to get min to 100 ms and 2048 gets rotations to units 
 //Right now I'm putting the motors at desired rpm for testing purposes 6380 or whatever number is after (2048 / 600) will change to rpm
leftShooterMotor.set(TalonFXControlMode.Velocity, motorSpeed);
rightShooterMotor.set(TalonFXControlMode.Velocity, -motorSpeed);

I’m not sure why the same issue happens in Phoenix Tuner, but the code above seems to explain the issue when not using the Tuner.

jackptx · February 10, 2022, 1:38am

Some possible sources of error:

Confirm your units for your feed forward terms is consistent with how you characterized. Your description closely matches the outcome of this error.
Manually calculate what your PID is doing with the value ranges to make sure they make sense. Characterization should have given you the proper feedback values.

agrimreaper48 · February 10, 2022, 2:14am

It appears that you are multiplying the rpm (5600) by 2048/600 which essentially maxes out the velocity. Max velocity on falcon is 6380 rpm.

Monarch49 · February 10, 2022, 8:10pm

1 seems to be the likely culprit looking back on my characterization. I had it in rotations. How would I get it to units / 100ms? It gives me the options: Meters, Feet, Inches, Radians, Rotations, Degrees. Thank you!

Monarch49 · February 10, 2022, 8:11pm

Yes, we have been setting the rpm manually to test our shooter’s capabilities. The problem is that when I set the motor to 5600 rpm the motor goes to 6380 rpm. Thank you for responding.

Monarch49 · February 10, 2022, 8:12pm

Does the velocity method take rpm? I thought it took units / 100ms. Thank you for the response.

Griffin · February 10, 2022, 8:17pm

Here is the relevant page in the CTRE docs that should answer all your questions: Motor Controller Closed Loop — Phoenix documentation

TytanRock · February 10, 2022, 8:19pm

If I had to guess, the main issue is your F gain.

github.com

hammerheads5000/2022RapidReact/blob/35b727016c1b0ca390f726dcc6ac8254b5a096c6/src/main/java/frc/robot/Constants.java#L95


    //PID shooter constants:
    public static int PID_LOOP_IDX = 0;
    public static int TIMEOUT_MS = 5;

    public static boolean PHASE_SENSOR = false;
    public static boolean INVERTED = true;
    public static double K_SENSOR_UNITS_PER_ROTATION = 2048.0;

    public static int SHOOTER_ERROR = 60;

    public static Gains kGains = new Gains(1.0, 0.0, 0.0, 1.0,  0,  1.0);


    public static double kS = 0.80192;
    public static double kV = 0.09327;
    public static double kA = 0.004018;
}

An F gain of 1 for talon FX is pretty high, for reference, the F gain in our Velocity Closed loop example is closer to 0.05.

github.com

CrossTheRoadElec/Phoenix-Examples-Languages/blob/d70cab6060617bbed5e207c2eaf8747af09a15f6/Java Talon FX (Falcon 500)/VelocityClosedLoop/src/main/java/frc/robot/Constants.java#L31


	 * Set to zero to skip waiting for confirmation, set to nonzero to wait and
	 * report to DS if action fails.
	 */
    public static final int kTimeoutMs = 30;

	/**
	 * PID Gains may have to be adjusted based on the responsiveness of control loop.
     * kF: 1023 represents output value to Talon at 100%, 20660 represents Velocity units at 100% output
     * 
	 * 	                                    			  kP   	 kI    kD      kF          Iz    PeakOut */
   public final static Gains kGains_Velocit  = new Gains( 0.1, 0.001, 5, 1023.0/20660.0,  300,  1.00);
}

I would zoom in on that, and reference the F gain in our documentation, specifically here Motor Controller Closed Loop — Phoenix documentation

Monarch49 · February 10, 2022, 8:48pm

This worked like a charm! Did you use SysID to find your kF value? If so what units did you use, we used rotations and clearly it didn’t work very well.

TytanRock · February 10, 2022, 8:56pm

Most of these examples were manually tuned with regards to PID, and calculated for F. I’ve never used SysID, so I can’t comment on how to use it with our products.

F in this case was calculated exactly following the guide here Motor Controller Closed Loop — Phoenix documentation - We ran the falcon at maximum output, took a self-test snapshot to measure ~20660 units/100ms, and calculated an F gain of 1023 / 20660 = ~0.05. I’d suggest you do a similar test for your mechanism, as the F gain is typically mechanism-dependent.

Oblarg · February 10, 2022, 9:34pm

The CTRE F gain is in units of 1 / (sensor ticks / 100ms). The corresponding SysId constant is kV, which in your case has units of volts / (rotations / second).

You can convert between the two, but you need to do some dimensional analysis that depends on your mechanism.

system · February 10, 2023, 9:35pm

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.