Talon SRX Position PID Control Issue

[Dan5190]

We have a crane style arm on our robot operated by two motors hooked up to 2 Talon SRXs. One of the talons is set in follow the other 'master' talon. We have a analog pot wired into the master talon data port to get the angle of the arm.

In the default state when the arm is not moving we have the master talon in position PID mode to hold the arm at it's current angle. When the arm is carrying a load it will sag down otherwise. We're using talon profile 0 for this PID control state. We want to move the arm to a specific angle we switch to profile 1 with a different set of PID parameters to move it.

The problem is that when we switch to profile 1 and then set the angle we want to go to the Talon seems to be in an invalid state and jerks the arm for split second before picking up the PID parameters and the arm angle setpoint we gave. I think what is happening is that during that split second it's trying to move the arm to position '0' on the analog pot. I don't think it's derivative kick or something with the I accum because I'm only setting a P for the PID parameters and also because when I set the setpoint to an angle higher than what the arm is currently at, the arm shoots down in the opposite direction for that split second then moves in the right direction.

Additional details in case it's relevant:
The valid range for the arm is 308-696 out of the pot's 0-1024 range which we have set as soft limits. When the operator moves the joystick to operate the arm we switch to PercentVBus mode then switch back to profile 0 position PID when they let go.


Here is the code that moves the arm to a new angle (uses pid profile 1):
setPID parameters:
P: 5.0 I: 0 D: 0 F: 0 Izone: 0 RampRate: 0 Profile: 1

public void startArmSetAngle() {
			if (controlMode != ControlMode.Position) {
				armCANTalonLeft.changeControlMode(ControlMode.Position);
				controlMode = ControlMode.Position;
			}
			configureSetAnglePID();
			armCANTalonLeft.set(angle);
		}
		// called every 20ms
		public void execute() {
			armCANTalonLeft.set(angle);
		}

private void configureSetAnglePID() {
		double p = prefs.getDouble("arm.set.angle.p", ARM_SET_ANGLE_P);
		double i = prefs.getDouble("arm.set.angle.i", ARM_SET_ANGLE_I);
		double d = prefs.getDouble("arm.set.angle.d", ARM_SET_ANGLE_D);
		double rampRate = prefs.getDouble("arm.set.angle.ramp.rate",
				ARM_SET_ANGLE_RAMP_RATE);
		int izone = prefs.getInt("arm.set.angle.izone", ARM_SET_ANGLE_IZONE);
		int profile = prefs.getInt("arm.set.angle.profile",
				ARM_SET_ANGLE_PROFILE);
		armCANTalonLeft.setPID(p, i, d, 0, izone, rampRate, profile);
	}

Here is the method that the default joystick command calls every 20ms to move the arm. If the joystick doesn't apply any power then it switch to position PID profile 0 to hold the arm at the current angle:
setPID parameters:
P: 1.8 I: 0 D: 0 F: 0 Izone: 0 RampRate: 0 Profile: 0

	public void moveArm(double power) {
		if (power > 0.05 || power < -0.05) {
			if (controlMode != ControlMode.PercentVbus) {
				armCANTalonLeft.changeControlMode(ControlMode.PercentVbus);
				controlMode = ControlMode.PercentVbus;
			}
			armCANTalonLeft.set(power);
		} else {
			if (controlMode != ControlMode.Position) {
				configureHoldPID();
				armCANTalonLeft.changeControlMode(ControlMode.Position);
				armCANTalonLeft.set(armCANTalonLeft.getPosition());
				controlMode = ControlMode.Position;
			}
		}
	}

	private void configureHoldPID() {
		double p = prefs.getDouble("arm.hold.p", ARM_HOLD_P);
		double i = prefs.getDouble("arm.hold.i", ARM_HOLD_I);
		double d = prefs.getDouble("arm.hold.d", ARM_HOLD_D);
		double rampRate = prefs.getDouble("arm.hold.ramp.rate",
				ARM_HOLD_RAMP_RATE);
		int izone = prefs.getInt("arm.hold.izone", ARM_HOLD_IZONE);
		int profile = prefs.getInt("arm.hold.profile", ARM_HOLD_PROFILE);
		armCANTalonLeft.setPID(p, i, d, 0, izone, rampRate, profile);
	}

[cbf]

We use the Talon in a similar manner to control our up/down elevator. By default, it's set to hold the elevator in place. But we also have manual joystick control (actually the triggers on an Xbox controller). We do have a small problem when switching from manual control (which is %VBus mode) to PID loop control -- the elevator falls a little bit before the PID loop takes control. But for us that seems to be because by the time we sample the position with GetEncoderPos, the elevator has already started falling (couldn't be more than 20 ms later). But we were surprised at how much it falls, so it could be we're experiencing something similar to what you're seeing.

Note -- we don't store the profiles in the Talon. We were concerned about storing the profile in the Talon in case we had to swap one out. So we just set them in code every time we enter kPosition mode.

We also are only using the "P" value. Didn't have time to tune the rest.

[GeeTwo]

• Have you tried setting the new setpoint first?
• Have you tried just setting the new set point, leaving the PID coefficients the same?
• Have you tried just setting the new PID coefficients, leaving the set point the same?

The behavior in these three cases should give you a much better idea of what the problem is, and maybe how to solve or at least work around it.

Also, if you're driving to a position that requires some torque to hold, a P-only solution is destined to droop to the level where the output voltage is enough to hold the drooped position. Are you adjusting for this by making the set point higher than you really want?

[ozrien]

I don't really understand what stops the first command from continually calling 'armCANTalonLeft.set(angle);' after power exceeds 5% (which is when the mode switches to voltage percent mode in the second snippet). Can you post the entire code?

[MaGiC_PiKaChU]

Our team experienced the same delay when we reset our encoders value directly on the SRX, and also when changing profiles.

I assume you are CAN connected, and we figured out this is what causes the delays on our side.

Edit : delays are about 100ms maximum, and didn't occur when we had only 2 SRX on the CAN wires (we now have 14)

[ozrien]

Originally Posted by MaGiC_PiKaChU

Our team experienced the same delay when we reset our encoders value directly on the SRX, and also when changing profiles.

I assume you are CAN connected, and we figured out this is what causes the delays on our side.

Edit : delays are about 100ms maximum, and didn't occur when we had only 2 SRX on the CAN wires (we now have 14)

I don't really understand this. How did you measure 100ms? 100ms between what and what?

The frame rates for everything is fixed, so it doesn't matter how many Talons there are on the bus. That's the design. The GetPosition() value is updated over CAN every 20ms (default). If the change-in-signal-value is a problem, you can speed up that frame for that particular Talon (see section 20.5). That's also used in the example in this post. Just keep an eye on your bus utilization.

I just tested this to see if I can reproduce what you're describing but it seems okay to me. 16 Talon robot (with 20 software Talon objects) so bandwidth doesn't seem to be a problem. Code below.

I recommend presetting the gain values ahead of time and just selecting the profile when you switch modes. Setting gains requires extra and unnecessary framing, but switching between two profiles has zero-footprint on the CAN bus since it's a signal in the fixed-rate control frame. And it's synchronous with all other control signals.

Your main loop might be really slow for other unrelated reasons. Try toggling an IO in your main loop and oscop-ing it (great opportunity to learn about oscilloscopes). How fast should your loop be running versus how fast does it actually run?

You can also try speeding up the control frame to see how that impacts things. That might help you understand what exactly is causing your problem.

The symptom reported by the OP sounds a lot like the wrong set point was initially set, but without the entire project it's difficult to be sure.

#include "WPILib.h"

class Robot: public IterativeRobot
{
	Joystick _joy0; //!< Just a single gamepad
	CANSpeedController::ControlMode _md; //!< track what mode we want for our test talon
	long _servoPos; //!< track our servo position for when we want to closed-loop
	bool _btn1; //!< last state of btn1 for on-press detection
	CANTalon * _tals[20]; //!< make a ton of talons.  16 of them are present on my test robot. ~78% percent bus utilization according to DS.
public:
	Robot() : _joy0(0), _md(CANSpeedController::kPercentVbus), _servoPos(0), _btn1(false)
	{
		/* create 20 Talons, 16 of them are present on my robot */
		for(int i=0;i<20;++i)
			_tals[i] = new CANTalon(i);
		_tals[1]->SetControlMode(_md); /* start in percent mode */
		_tals[1]->SetSensorDirection(true); /* sensor and motor are out-of-phase, so flip the sensor */
		/* speed up the CAN frame that reports sensor position, so calling GetPosition() is as fresh as possible */
		_tals[1]->SetStatusFrameRateMs(CANTalon::StatusFrameRateFeedback,1); /* 72% => 78%, added 6% to CANBus Util*/
	}
	void TeleopInit()
	{
		/* empty */
	}
	void TeleopPeriodic()
	{
		/* grab joystick inputs */
		double y = -1 * _joy0.GetY(); /* moving-axis-up should be positive now */
		bool btn1 = _joy0.GetRawButton(1);

		/* btn1 => servo pos1 */
		if(!_btn1 && btn1){
			/* change to position mode */
			_md = CANSpeedController::kPosition;
			_tals[1]->SetControlMode(_md);
			/* calc where to servo to, say 5000 units from wherever we are now.
			 * An absolute value can be used too if that makes sense. */
			_servoPos = _tals[1]->GetPosition() + 5000;
		}else if((y > 0.05) || (y < -0.05)){
			/* y axis is being used, change to throttle mode */
			_md = CANSpeedController::kPercentVbus;
			_tals[1]->SetControlMode(_md);
			/* y axis is used below */
		}else{
			/* y axis is in the dead band.  So servo to last known position */
			if(_md == CANSpeedController::kPercentVbus){
				/* we are in throttle mode, better go back to servo where we are */
				_md = CANSpeedController::kPosition; /* change to pos mode */
				_tals[1]->SetControlMode(_md);
				/* calc where to servo to */
				_servoPos = _tals[1]->GetPosition();
			}else{
				/* we are servo-ing to our last position */
			}
		}

		/* based on selected mode, apply the correct target value. */
		if(_md == CANSpeedController::kPercentVbus){
			_tals[1]->Set(y);
		}else if(_md == CANSpeedController::kPosition){
			_tals[1]->Set(_servoPos);
		}

		/* save button state for detecting on-press */
		_btn1 = btn1;
	}
};

START_ROBOT_CLASS(Robot);

[MaGiC_PiKaChU]

Originally Posted by ozrien

I don't really understand this. How did you measure 100ms? 100ms between what and what?

The frame rates for everything is fixed, so it doesn't matter how many Talons there are on the bus. That's the design. The GetPosition() value is updated over CAN every 20ms (default). If the change-in-signal-value is a problem, you can speed up that frame for that particular Talon (see section 20.5). That's also used in the example in this post. Just keep an eye on your bus utilization.

the delay is between the time we gave a command (setposition(0)) to reset our encoders during auto and the time the talon actually returns a position of 0. I don't know if other teams experienced this, I was just sharing the issue

Edit : We have one sensor directly in the SRX port for 12 of our talons

[ozrien]

Are you using getEncPosition() or getPosition()? The former only updates every 100ms, which explains what you saw. The later every 20ms.

The signal is updated in the Talon pretty quick. Say 100us after receiving the can frame to be safe. And the setPosition(0) immedietely sends a can frame to the talon.

But the getters only get periodic updates over CAN Bus. If that's a problem you can increase the frame rate of the corresponding message.

[MaGiC_PiKaChU]

Thanks for the feedback! Really useful