Help with Hold Position using Motion Magic on a Pivot Arm

Technical Java
1

W are having trouble keeping an arm in position once it pivots to the setpoint. We were able to successfully tune the extend/retract of our arm using Motion Magic with no issues. We can pivot to a set position using Motion Magic; however, we do not understand how to hold that position. We are using a Falcon 500 and the built-in encoder. I am not sure what else to include besides the following two code snippets. New to Java so please go easy on me.

public void setPivotMotionMagic(double targetRotations) {
      double targetTicks = targetRotations * 80 * 2048;
      double horizontalHoldOutput = 0.15;
      double arbFeedFwdTerm = getFeedForward(horizontalHoldOutput);
      //pivotMotor.set(TalonFXControlMode.MotionMagic, targetTicks);
      pivotMotor.set(TalonFXControlMode.MotionMagic, targetTicks, DemandType.ArbitraryFeedForward, arbFeedFwdTerm);

We use ArbitraryFeedForward and calculate based on a horizontal hold output which we found using Phoenix Tuner and set at 0.15 of motor power.

private double getFeedForward(double horizontalHoldOutput) {
      double pivotSensorPosition = pivotMotor.getSelectedSensorPosition(0);
      double pivotCurrentAngle = (pivotSensorPosition/80*360/2048);  //lowest position is 0 via sensor; needs 45 deg offset
      // double theta = Math.toRadians(90 - (pivotCurrentAngle + 45));  //angle of elevation for arm; 90 is max hold
      double theta = Math.toRadians(56 - pivotCurrentAngle);
      double gravityCompensation = Math.cos(theta);
      double arbFeedFwd = gravityCompensation * horizontalHoldOutput;
      return arbFeedFwd;
    }

Full code is GitHub - FRC8167/PizzaPlanetCLAW-H

2

When you say trouble keeping it in position, what exactly is happening? E.g. is the arm oscillating, sagging, going up?

3

The code you have, doesn’t really explain the settings you are using for the PID to make this happen. If I had to guess I would say that your P value is too small. Either that or your ArbFeedFwd isn’t tuned properly.

If you want to test the arbFeedFwd value you could put the arm in Percent Output mode and apply the calculated arbFeedFwd value for whatever position your arm is in. In all positions it should basically hold still.

One more thing, are you calling set with the ArbFeedFwd value multiple times? You have to call it every cycle so that the motor controller has the new ArbFeedFwd value throughout the motion.

4

Slowly sags downward.