Strugging with doing PID position control on TalonSRX

Technical Java
1

Hello evereyone!
My team is having trouble running PID position closed-loop control on our drivetrain with TalonSRX to get to a certain position. We tried breaking the code into smaller and smaller pieces and ended up in opening a new blank robot project with only the required lines for the pid written. (as attached)
When we run the PID, our robot just drives forward without stopping. we printed both the current position of the talon and the setpoint it had received to the SmartDashboard and could clearly see that it just skips by the target and keeps moving forward.
Please advise. Is anything in our way of using PID on TalonSRX wrong? Is there something else going on?

Thanks a lot in advance, Aric.

P.S. We also tried running the code that’s located in the teleopPeriodicin teleopInit (meaning it only ran once and not periodically) and results were the same.


package org.usfirst.frc.team3339.robot;


import com.ctre.CANTalon;
import com.ctre.CANTalon.FeedbackDevice;
import com.ctre.CANTalon.TalonControlMode;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Robot extends IterativeRobot {

	public static OI oi;
	
	private CANTalon rearLeft = new CANTalon(3);
	private CANTalon rearRight = new CANTalon(1);
	private CANTalon frontLeft = new CANTalon(2);
	private CANTalon frontRight = new CANTalon(0);
		
	
	@Override
	public void robotInit() {
		oi = new OI();
		
		frontRight.reverseOutput(true);
		rearRight.reverseOutput(true);
		frontLeft.setFeedbackDevice(FeedbackDevice.QuadEncoder);
		frontLeft.reverseSensor(true);
		frontLeft.configEncoderCodesPerRev(360);
		frontLeft.setPID(0.2,0,35,0.5115,0,0,0);
		frontLeft.configMaxOutputVoltage(12);
		
	}

	@Override
	public void disabledInit() {

	}

	@Override
	public void disabledPeriodic() {
		Scheduler.getInstance().run();
	}

	@Override
	public void autonomousInit() {
	}
	
	@Override
	public void autonomousPeriodic() {
		Scheduler.getInstance().run();
	}

	@Override
	public void teleopInit() {
		frontLeft.setPosition(0);
	}
	
	@Override
	public void teleopPeriodic() {
		Scheduler.getInstance().run();

		frontRight.changeControlMode(CANTalon.TalonControlMode.Follower);
		frontRight.set(2);
		rearRight.changeControlMode(CANTalon.TalonControlMode.Follower);
		rearRight.set(2);
		rearLeft.changeControlMode(CANTalon.TalonControlMode.Follower);
		rearLeft.set(2);
		frontLeft.changeControlMode(TalonControlMode.Position);
		SmartDashboard.putNumber("Setpoint", frontLeft.getSetpoint());
		SmartDashboard.putNumber("Position", frontLeft.getPosition());
		frontLeft.set(5);
	}

	@Override
	public void testPeriodic() {
		LiveWindow.run();
	}
}
2

Watch the closed-loop error (note that it will be in native units, not rotations; the inconsistency in units is one reason to avoid calling configEncoderCodesPerRev() and instead to just handle unit scaling yourself). Possibly, your motor and encoder are out-of-phase, meaning motor positive is encoder negative. This results in a positive feedback loop, which will simply run off to max output.

You should always start with only P gain when initially trying to get a loop working (and really, a simple P loop suffices for ~95% of what you want to do in FRC); your D gain is awfully high, regardless, compared to P. Additionally, why do you have a feedforward gain set in position mode? I do not know if this actually does anything in position mode, but if it does, it will probably (as in other control modes) introduce a constant output proportional to your setpoint, which is not appropriate and could certainly cause the behavior you describe.

Perhaps more pertinently, though, you really shouldn’t use a position servo with a step input on a drivetrain to move a specified distance; you will immediately output max voltage and likely slip the wheels while accelerating (thus ruining your positional accuracy), and also probably overshoot and oscillate upon reaching the setpoint. Consider using a low-speed velocity servo or a motion profile; these are both better solutions.

3

Hey Eli!
We removed the setting of the F value and it indeed helped.
Also, we are working on implementing the rest of your tips.

Thanks a lot for your help!