Bugged Smart Motion Position Control

[JulianOrteil]

Attempting to use position control in the Java Smart Motion example is giving undesirable offsets for our arm mechanism.

We are using the example as-is, except for the following changes:

• m_encoder is the through-bore encoder in absolute mode
• m_encoder is inverted (m_encoder.setInverted(true))
• integral of PID is set to 0

Here is what we observe:

• Smart Motion does not respect the absolute mode of the encoder. Smart Motion's zero point is where the arm starts regardless of the degree angle reported by the encoder. For example, our arm naturally starts at about 10 degrees. When we set the setpoint on the dashboard to go to 0, the controller sends the arm back to that 10 degree starting position, not the literal zero point of the encoder. If our arm starts at 8 degrees, Smart Motion sends the arm back to 8 degrees.
• Smart Motion has some scaling offset applied, like our setpoint is being multiplied by some unknown value (i.e. setpoint * 1.5). For example: when we set the setpoint to 10 degrees, the arm goes to 15. When we set it to 45, it goes to 60. 90 goes to 115. This goes beyond our proportional being too low because we increased it to the point of uncontrolled oscillation where it oscillated around these offset values (15, 45, or 115, for example).

Is there something we missed in the documentation or is this a bug in the firmware we can't fix? Nothing we've done to try and remedy this has worked (i.e. trial/error PID values, correct for this unknown scaling issue (i.e. (setpoint - start angle) / 1.5))

[lost1227]

Hi, I think you're missing a call to SparkMaxPIDController.setFeedbackDevice()​.

According to the documentation on this method:

The default feedback device in brushless mode is assumed to be the integrated encoder and the default feedback device in brushed mode is assumed to be a quadrature encoder.

I'm guessing you're using a brushless motor, and the spark max is running the smartmotion controller off of the relative encoder supplied by the integrated hall effect sensor. (The scaling offset is likely the ratio of the gearbox on your motor)

Note that I've never personally tried SparkMaxPIDController.setFeedbackDevice() to set a spark max to run PID on an absolute encoder, so I'm not sure how well it would work. You might run into issues related to the flex and backlash that comes with having the gearbox and mechanism stand between the motor and the encoder.

On our team, we're just using the absolute encoder to zero our relative encoder at robot startup, then running all of our PID on the relative encoder. I'd recommend this approach, especially if you try the setFeedbackDevice approach and notice any issues.

[JulianOrteil]

Hi @lost1227

Those are some great points. We'll definitely give it a shot tonight when our team meets. And apologies for not mentioning how our mechanism is structured: we are using a standard Neo with a gearbox that has a ratio of about 229:1. Our encoder, the through-bore from REV, is on the pivoting shaft of the arm, so backlash isn't going to be an issue.

I will confirm that setFeedbackDevice does work as expected in absolute mode as we also use REV's MAXSwerve modules on our robot which include the through-bores.

I'll update this with any news on our findings later tonight. Thank you!

[terbos90803]

I have to agree with @lost1227. Putting the absolute encoder on the final output shaft is a great idea for knowing exactly where the arm is, but it's a terrible idea to use that position directly for SmartMotion control.

If you do the math, the absolute encoder has a resolution of 1024 ticks per revolution, which works out to a bit under 3 ticks per degree.

With your 229:1 gear ratio, the motor will go through 229 degrees of rotation for every degree of the arm. This means that the motor's core will have to rotate through 81 degrees just to notice that the abs encoder has changed 1 tick. That's 10x worse resolution than using the motor's internal encoder.

Also, if there are gears, there is lash. It might seem small, but any lash at all between the motor and the encoder will show up as instability in a closed loop control like SmartMotion.

Bottom line:

• Use the NEO's internal encoder for SmartMotion control.
• Use the abs encoder to calibrate/reset the internal encoder at startup. You can set the internal encoder to any position count, it doesn't have to reset to just zero.

As for your issue of the commanded position not matching the expected result, you might review the scaling factor you're setting into the SparkMax. It should include the entire ratio stack from the motor to the output. e.g. 229:1 (although I think Rev prefers the reciprocal IIRC).

[JulianOrteil]

@terbos90803

Based upon your comments, how our team has been trying to run this mechanism has been wrong this entire time then. Let's essentially ignore everything we're observing right now since our thinking has been incorrect. If we fix our thinking, it might fix our observed issues.

Please bear with me as I would like to take advantage of your knowledge if you don't mind.

Once again, assuming we use the Smart Motion example here as a basis, our code should do the following:

// m_encoder already exists and is set to the motor's internal
// based on @lost1227's input, the PIDController should already be set to the internal encoder
AbsoluteEncoder m_absEncoder = m_motor.getAbsoluteEncoder(Type.kDutyCycle)
...
// robotInit
m_absEncoder.setPositionConversionFactor(360);  // degrees, absolute mode reports between 0 and 1, multiply by 360 and we have detected degrees
m_encoder.setPositionConversionFactor(1 / (229 * 360));  // gear ratio * degrees of rotation; relative mode increments, so it needs to be reciprocal
m_encoder.setPosition(m_absEncoder.getPosition());
...
// robotPeriodic
double setPoint = mySetpointSupplier();  // setpoint in degrees
m_pidController.setReference(setPoint, CANSparkMax.ControlType.kSmartMotion);

Does this seem correct?

Unfortunately, the storm roaring across the Midwest has forced us to cancel meeting tonight and tomorrow, so we can't test either of your recommendations until at least Saturday.

Final question: should Smart Motion always use the internal encoder (let's think beyond the current game and this arm mechanism of ours), or is its ideal sensor something else?

Much appreciated!

[lost1227]

That looks pretty close to what we're doing. You'll want to fine-tune that gear ratio: we've found that (at least with our construction tolerances) there's enough play to create a significant difference between the theoretical and actual ratios. It's easy enough to adjust: just log both the relative and absolute encoders to the shuffleboard (or just using System.out.println), move the mechanism some amount, note the difference in readings, and calculate the adjusted ratio.

The only other note I have is related to the zero position and the fact that, unlike the absolute encoder, the relative encoder doesn't wrap. At our last meeting, we noticed that if our mechanism is at all past our configured zero, then the relative encoder gets zeroed at +1 revolution more than it should. For example, if we configure our zero, then the mechanism is pushed a bit further back past the zero so that the encoder reads 358°, then the relative encoder gets zeroed to 358° when it should really read -2°. This can cause the smartmotion to try to turn the mechanism all the way around back to 0, colliding with the hard stops (and potentially damaging the mechanism). There's probably a way to fix this with SparkMaxPIDController.setPositionPIDWrappingEnabled, but we couldn't figure it out. Instead, we decided on an approach that always assumes the mechanism is within 180° of the zero, and use something like:

double position = m_absEncoder.getPosition();
if(position > 180) {
    position = position - 360;
}
m_encoder.setPosition(position);

With regards to your second question about always using the internal encoder for Smart Motion, I'm not really sure. I defer to @terbos90803's experience.

[terbos90803]

Yes, SmartMotion should always use the internal encoder. It's the closest one to the motor (because it is the motor) and thus the best at managing closed loop control without the possibility of lash.

[JulianOrteil]

@terbos90803 @lost1227

Sorry for the delayed update on this. Replacing a component on the robot ended up taking 3 days instead of 3 hours, so we only just got to testing this today.

We were able to get everything working as expected. As @lost1227 pointed out, our tolerances are causing significant differences between what we tell our arm to go to and where it actually ends up (upwards of 10+ degrees). At least the difference is consistent, so big improvements there. We're going to have to skip calculating the actual ratio for now and just compensate for it in what degree we tell the arm to go to. We only have 4 meetings until the competition next week, and we have yet to get our autonomous routine working properly--so we need to focus on that and get proper drive practice.

Our final code ended up looking pretty similar to the above, but here it is for anybody else who might be encountering this issue in the future (I don't have it in front of me, so this is from memory--sorry for any mistakes):

// Using the Smart Motion Example as a base...
// m_encoder already exists and is set to the motor's internal
// based on @lost1227's input, the PIDController should already be set to the internal encoder
AbsoluteEncoder m_absEncoder = m_motor.getAbsoluteEncoder(Type.kDutyCycle)

// our encoders are mounted "backwards" to how they count
// setting the motor's inversion also sets m_encoder's inversion
m_motor.setInverted(true);
m_absEncoder.setInverted(true);
...
// robotInit
m_absEncoder.setPositionConversionFactor(360.0);  // degrees, absolute mode reports between 0 and 1, multiply by 360 and we have detected degrees
m_encoder.setPositionConversionFactor(360.0 / gearRatio);  // m_encoder increments once **per rotation** of the motor

// thanks a ton to @lost1227 for this code below...
double position = m_absEncoder.getPosition();
if(position > 180) {
    position = position - 360;
}
m_encoder.setPosition(position);

...
// robotPeriodic
double setPoint = mySetpointSupplier();  // setpoint in degrees
m_pidController.setReference(setPoint, CANSparkMax.ControlType.kSmartMotion);

As for the code, @lost1227 thank you a ton for that position code you supplied. We encountered exactly what you described, implemented what you gave there, and it worked out of the box. Couldn't thank you enough.

Thank you a ton as well @terbos90803 for clarifying a lot of our confusion. This is our first year trying a mechanism like this AND our first time using REV's ecosystem, so thank you for the helping hand.

On behalf of Team 6166, Jules