MotionMagic PID Tuning Questions

I’ll preface this with the hardware used:

775pro -> 100:1 versaplentary (w/ SRX mag encoder) -> 2:1 gearset moving the arm.
1 rotation (4096units) = the full range of motion of the arm (180 degrees)

This was my first time tuning a PID controller with a TalonSRX / DC motor in general.
Since 1 full rotation provides the entire range of motion, I used a relatively low accel / cruise velocity.
I will probably end up increasing it but it works for now.
200units/100ms accel; 400units/100ms cruise

I had planned on using Ziegler–Nichols method of PID tuning, but since MotionMagic creates a motion profile, I found it to be quite obvious that this would be impossible.

That being said, I raised P until there was a slight oscillation, and tuned D until it no longer oscillated and hit on the exact unit at least 95% of the time.

In this case, P turned out to be 10, and D turned out to be 210. F and I were left at 0.
With these settings, I was able to rotate from 0-4096 units and back with little to no deviation; sometimes it would miss by ± 1 unit.

Note this tuning was done with 0 load on the motor.

This brings me to my first question

  1. Was this a viable method of PID tuning / do these numbers seem realistic? Am I missing anything?

Since this is controlling an arm, I plan on using Arbitrary FeedForward to compensate for the force of gravity.

From my understanding, assuming the voltage applied through ArbFeedForward is added to the PID output, the PID tuning should work as if there was no load on the motor assuming the compensation is accurate.

To calculate feedforward, I was planning on finding the voltage required to hold the arm parallel to the ground, then multiplying that by the cosine of the angle of the arm & plugging it back into the feedforward calculation as the arm rotates.

  1. Is this how Arbitrary Feedforward (in theory) should work? Has anyone tried this and found success?

This is a sub-question of question #2:
I read that 1023 represents full voltage.

  1. If, for example, the voltage required to stall the arm was 1v, would my feedforward value be
    (1023/12) * cos(x) ?

So, you’ve got a few things funky here. One of your biggest problems is this:

I recommend hooking up your motor to the actual mechanism in question and doing the tuning there. CTRE provides a very useful guide on how to tune the Motion Magic PID that I suggest you read and follow, then add your arbitrary feed-forward afterwards. I don’t see anything theoretically wrong with your arbitrary feed-forward calculation (and your understanding of the principle is correct), but the units for arbitrary feed-forward are actually motor throttle from -1 to 1. (Your confusion there is understandable.)

The numbers you get will depend on many things, namely the physical characteristics of your mechanism, where the encoder is located relative to the motor and the mechanism, and other variables. However, in my unprofessional opinion, a P of 10 and D of 210 seems a bit high.

Okay, so in the case of it being -1 - 1, the feedforward value would just be 0.083, correct?

Also, If I were to run those values in normal position cloosed loop mode, those gains would obviously be way too high, but it is smooth with MotionMagic.

I will read through their guide, thanks for letting me know.

Motion Magic is not traditional PID on position. Rather, it is real-time motion profiling, where your mechanism is following a trajectory. With Motion Magic, you should be utilizing both feedforward and feedback. Here’s a rough explanation of how Motion Magic works:

  1. Motion Magic takes your current position, current velocity, goal position, velocity constraints and acceleration constraints, and determines a smooth profile for your mechanism to follow. This profile consists of desired position and desired velocity over time.
  2. Motion Magic looks at the motion profile generated and determines how fast your mechanism should be moving right now. Using the F gain, it scales that velocity to a rough motor output.
  3. Motion Magic then looks at the mechanism’s current position, and uses the motion profile to determine where it is supposed to be right now. This is where PID is used to correct for any positional error, and this error-correcting output is added to the output calculated in #2.

This process is repeated periodically until the mechanism reaches the target. I would check out CTRE’s guide that was posted above for a good explanation of how Motion Magic works and how to tune it.

In terms of arbitrary FF, you are on the right track. On top of the output calculated in #2 and the output added in #3, the specified arbitrary FF is also added to the final output. So, if you need to command a bit more output to keep your arm up, you pass that in as an arbitrary FF value (from 1 to -1). As you’ve done, the arbitrary FF value for an arm is proportional to cos(theta), where theta is the angle from the horizontal.

To clarify, by “no load on the motor” do you mean with nothing connected to the gearbox output or with the motor moving the mass of the arm but not pushing against any additional load?

nothing connected to the gearbox output

You can’t tune PID with no load on the motor. PID is all about the behavior of the system under load; the amount you have to dampen and compensate for disturbances, inertia, etc. is absolutely dependent on the system load. You can’t realistically do it “in advance” in FRC; you need the full system to be built and connected.

1 Like

It may be a dumb question but… how do you record the maximum sensor velocity at 100% motor output when the range of motion is limited (like for an arm)?

See Sensor Preparation and Motion Magic documentation as reference:

  • Record the maximum sensor velocity (position units per 100ms) at 100% motor output.

I guess it’s easy to record the maximum sensor velocity for a drive train while driving around. But how should it be recorded for something like an arm or an elevator?

One thing you can do is that the voltage-speed curve of FRC motors is effectively linear, so you could do testing at half instead of full throttle and then double your velocity figure.

1 Like

You could also tune at part throttle and ask yourself what the risk-reward is for running a motion-limited mechanism at full speed vs. giving it some margin of error. I mean, as a programmer I strive for full employment on the mechanical subteams, but occasionally they get annoyed when I tear apart yet another of their works of art :slight_smile:

5 Likes

I was a bit confused on this. Is it the cosine of the angle or the cosine of radians? Once I switched the angle of the arm to radians it gave us the range from 0 to 1 that was needed to linearize the feedforward.

 // take the horizontalHoldOutput and linearize it by multiplying it by the cos of the angle of the arm
 public double getFeedForward() {
  double radians = Math.toRadians(getAngle());
  double feedForward = horizontalHoldOutput * Math.cos(radians);

  if(tunable) {
    SmartDashboard.putNumber("Lift Angle", getAngle());
    SmartDashboard.putNumber("Lift FeedForward", feedForward);
  }
  return feedForward;
}

Math.cos() takes an input of radians, so you definitely do want to convert from degrees (assuming getAngle() returns degrees) as you’ve done in your code above.

Thank you for verifying.

For anyone attempting this, it’s really helpful to print the values in a tool like SmartDashboard to ensure your theory is being applied correctly! You’ll want to print out the angle of the arm, so you can test that your code is reading the correct angle when you actuate to a position. You’ll also want to print out the gravityCompensation so you know you are correctly linearizing the system. Then multiply the gravity compensation to the “minimum power needed to hold the arm up” and you have feedforward.

public double getFeedForward() {

  // get the radians of the arm
  // getAngle() returns degrees
  double theta = Math.toRadians(getAngle());

  SmartDashboard.putNumber("Angle", getAngle());

  // get a range of 0 to 1 to multiply by feedforward.
  // when in horizontal position, value should be 1
  // when in vertical up or down position, value should be 0 
  double gravityCompensation = Math.cos(theta);

  SmartDashboard.putNumber("Gravity Compensation", gravityCompensation);

  // horizontalHoldOutput is the minimum power required to hold the arm up when horizontal
  // this is a range of 0-1, in our case it was .125 throttle required to keep the arm up
  double feedForward = gravityCompensation * horizontalHoldOutput;

  SmartDashboard.putNumber("Feed Forward", feedForward);

 return feedForward;

}

Sanity Logic
We are linearizing the system to compensate for gravity. When the Arm is horizontal, you should be getting a gravityCompensation value of 1. This means you’ll multiply your horizontalHoldOutput by 1 since that’s when the arm is at it’s strongest load and needs the feedforward the most. When the arm is all the way down or all the way up, the gravityCompensation value should be 0. Since you should need 0 power to hold an arm hanging straight down, you multiply 0 to the horizontalHoldOutput resulting in 0 feedForward.

2 Likes

Great, easy-to-read, well-commented code snippet. Thanks for sharing!

1 Like

You’re welcome. It’s frustrating when you only get a piece of an answer or snippet of theory.

What Does your getAngle() method look like?

it’s ugly. since horizontal needs to be 0 degrees, our arm starts at -30 degrees. At -30 degrees our encoder reads 0. at horizontal, it reads 1000. We found the ticksPerDegrees to be able to translate our encoder value into degrees, but had to also subtract our original position from the value. Hope it makes sense:

 // return the angle of the arm based on the current encoder value
 public double getAngle() {
  int currentPosition = getPosition();

 // divide the encoder position when arm is horizontal by the angle displacement
 // so if you moved the arm 30 degrees and read 1000 ticks, it would be 1000/30 ticks per degree
 // subtract horizontalAngleDisplacement to make the horizontal postion 0 degrees
 double ticksPerDegree = horizontalPosition/horizontalAngleDisplacement;
 double angle = currentPosition / ticksPerDegree - horizontalAngleDisplacement;
 return angle;
}