Swerve Modules flip 180 degrees periodically/conditionally

Hello, Chief Delphi, my team just started using swerve modules this year and we have problems. Note: We’re using WPILib’s swerve module code so that we don’t have to do the calculations ourselves. Every time our robot rotates 360 degrees on its axis(clockwise or counterclockwise), the swerve modules turn 180 degrees opposite the direction of rotation. In addition, whenever the robot moves 360 degrees of an circle, the same phenomenon happens. We are also currently using WPI’s optimize method to make sure we turn efficiently every time, and we are using the ADIS 16470 IMU to obtain our angular rotation relative to startup position. We’re having trouble figuring this problem out and we may need some help. If you need code samples, feel free to ask. Thank you.

What motors are you using? And also, yes it’ll be easier if you can just post a github link to your code.

We are using Falcon 500s, two per module. By the way, I should’ve mentioned that we’re only using two swerve modules this time around on opposite vertices of our square frame.

So I’m fairly certain the issue is that, you are directly using the output of the optimize function for the Falcon pid.

The optimize function assumes that the pid controller is continuous, which means that if you are at 359 degrees and set a target of 1 degree, it assumes the controller will be able to tell the module to move 2 degrees forwards and not 358 degrees backwards (which is how the wpilib pid controller functions).

However CTRE onboard pid controllers are not continuous, if it’s at a setpoint at 359 and you tell it to go 1, it will rotate 358 degrees backwards instead of 2 degrees forwards. If you want it to go to 1 you actually need to tell it to go to 361 degrees. To do this you need a custom optimize function.

We found this same issue and adapted the optimize function using some code out of 1323’s library.

Line 46 you can see where we use the custom function, and the function itself is in the lib/util folder here:

Ignore the reademe and etc, it’s not complete yet, but the actual code itself works as intended for a falcon swerve.

3 Likes

You might also look at this. I don’t know if it applies or not…

Thx so much. My teammate is talking with our programming mentor rn and I think they have a lead on the problem.

Thanks! We borrowed your code and it worked perfectly!

1 Like

I’d like to clarify some of your terminology. Continuous means that if you are at 359 and set the desired angle to 1 degree, it will rotate 358 degrees to one degree. Because the PID IS continous, to go to 360 degrees in the shortest direction, you’d have to call it 360 degrees.

A compass is discontinous - it jumps from 359 to 0.

So I believe you’re reversing the terminology.

I’m basing this on the definitions used in math. If there’s another interpretation and I’m wrong, please let me know. I don’t want to teach this incorrectly.
image

3 Likes

Maybe a more intuitive term would be unwinding? We ran into this problem as well in 2020 and called it that.

EDIT: Also, this is semi related but you can also use syncQuadratureWithPulseWidth() to make sure your reported position is always 0-4096 (assuming your gearing is 1-1)

Change the quadrature reported position based on pulse width. This can be used to effectively make quadrature absolute. For rotary mechanisms with >360 movement (such as typical swerve modules) bookend0 and bookend1 can be both set to 0 and bCrossZeroOnInterval can be set to true. For mechanisms with less than 360 travel (such as arms), bookend0 and bookend1 should be set to the pulse width values at the two extremes. If the interval crosses over the pulse width value of 0 (or any multiple of 4096), bCrossZeroOnInterval should be true and otherwise should be false. An offset can also be set.

Works for me!

1 Like

This is not necessarily true based on the continuous mode in the WPILib PID Controller.

  /**
   * Enables continuous input.
   *
   * <p>Rather then using the max and min input range as constraints, it considers them to be the
   * same point and automatically calculates the shortest route to the setpoint.
   *
   * @param minimumInput The minimum value expected from the input.
   * @param maximumInput The maximum value expected from the input.
   */
  public void enableContinuousInput(double minimumInput, double maximumInput) {
    m_continuous = true;
    m_minimumInput = minimumInput;
    m_maximumInput = maximumInput;
  }

Taken from PIDController.java.

The PID Controller essentially treats the mininimumInput and the maximumInput as the same point when continuous mode is enabled.

Let’s say our current measurement is 359 degrees and our desired state is 0 degrees. If continuous mode was enabled, then 0 degrees is treated as the same point as 360 degrees, thus the “output” of the controller would move the mechanism straight to 360 degrees.

We can look at the internal code to confirm this.

/**
   * Returns the next output of the PID controller.
   *
   * @param measurement The current measurement of the process variable.
   */
  public double calculate(double measurement) {
    m_measurement = measurement;
    m_prevError = m_positionError;

    if (m_continuous) {
      m_positionError =
          MathUtil.inputModulus(m_setpoint - measurement, m_minimumInput, m_maximumInput);
    } else {
      m_positionError = m_setpoint - measurement;
    }

Taken from PIDController.java.

Looking at the same scenario from before measurement = 359, m_setpoint = 0, m_minimumInput = 0, m_maximumInput = 360.

If continuous mode is disabled then m_positionError = 0 - 359 = -359 which would move a mechanism 359 degrees CW to 0 (assuming CCW positive). This is the non-optimal path in our situation.

If continuous mode is enabled then the error (m_setpoint - measurement = -359) is wrapped[placed in] between our min and max input by inputModulus. Skipping the internal math (it can be thought of as analogous to adding/subtracting 360/2pi to get a different angle with the same sin/cos on a unit circle) we get m_positionError = 1.0 thus the mechanism would move 1 degree CCW (assuming CCW positive) to get to 0/360 degrees. This is a more optimal path than moving 359 degrees CW to get to the same point.

1 Like

And just to add onto what CTT said, CTRE’s terminology also concurs.

They classify encoders that wrap around as continuous, and encoders that keep counting as non-continuous.

1 Like

I believe you mixing terminology in that case. By continuous, they are stating that it can be continually rotated.

Usually, with an encoder, a ‘wrap around’ encoder is called an absolute encoder because it represents an angle (indirectly via the voltage), and a non-stop counting encoder is called incremental encoder.

The case that wpiLib has named it that way should probably be the standard here, just so we don’t drive confusion.

But I think what I’m seeing here is there is just no agreed-upon standard. So I’m going to drop it, because it’s all semantics anyway =).

2 Likes