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