Wrap-around with rev spark maxes

Heyo! This is my team’s first year building our own swerve drive and we’re having some issues~ We notice that when setting the target for our motors our pid works fine, but since we’re aiming for a rotational target, our controllers are making an odd reaction.

It seems that they dont have a wrap-around feature, when we ask the controller to move from 3.1, to -3.1 (a small angle) it instead wraps in reverse and takes the long route all the way through 0 when it should take a much shorter route.

We noticed rev did not seem to have methods for this type of thing so we experimented with using a sum that would take the difference between two rotation2ds as a value in radians and append it but we seem to be loosing radians and steps and overall it feels very clunky… Has anybody else encountered a similar problem and how did you fix it?

We’ve recently looked at just ditching rev PID for the steering alltogether, and simply piping values over can to use the wpilib pid controller witch does have an angular wrap around feature.

From the Rev trello:

1 Like

That’s just a feature request though, with no activity since November.

I have been linked to the trello board a LOT and im sorry to say that even features they move to done are not implemented yet :c

Any chance rev moved the other trello card they had for automatic zero on endstop rise?

Edit: found it, Trello

really wish these were ready! the 112ms delay is causing issues when zeroing for us rn

Not sure what you mean by this? Calling encoder.setPosition() is very fast.

Which ones?

Oh sorry, suppose its something else than, too many tabs open, the delay is only for velocity control not sensors. Thoughts on NEOs and Spark Max - #65 by dyanoshak

As for other features, the zero-on-lock was moved to done but iirc it got moved back to feature and a few other things were moved around a lot especially method renames

You’re right that something is going to have to mediate between the two systems. I’m going to use a circle with angles between -180 and 180 as an example, since I’m not sure what your units are in.

One way to do this would to have a target angle and figure out how to map it to a sensible instruction for the Spark. For now, let’s say that target angle is 45 degrees. Once you have your target, you query your Spark for it’s current angle - since it’s counting continuously, perhaps it says it is at 400 degrees, since it has already rotated more than once counter-clockwise beforehand.

We anchor ourselves at 400, and say that from that position the only sensible instructions are between 220 and 580 degrees (400-180 and 400+180 degrees respectively). Next, we translate our target angle into this range, via something like the following code:

if (value < lowerBound) {
    value = upperBound
            + ((value - lowerBound) % (upperBound - lowerBound));
} else if (value > upperBound) {
    value = lowerBound
            + ((value - upperBound) % (upperBound - lowerBound));

this maps our 45 degree target to 405 degrees. We can now call setReference on the Spark with a target of 405 and expect the onboard PID to behave sensibly.

However, there is still one significant source of error here - the ratio between motor rotations and rotations of your swerve module. Unless that ratio is relatively accurate, you are going to build up error continuously if your modules keep turning in one direction. Potential solutions:

  • Use an external encoder mounted directly to the steering shaft of the swerve module, such as a CANCoder.
  • Take accurate measurements of the ratio between motor rotations and swerve module rotations. Zero everything out, then rotate the module 20 or 50 times in one direction, then measure how many motor rotations that maps to.
  • Write some software to periodically “un-twist” your modules so that they prefer to stay relatively close to the zero point and away from large #s of rotations in a single direction.

Our current solution has been to just handle the delta angle on our own with a method, this way we can append the delta angle witch wpilib handles correctly to the continous count on the motor controller. I think we have similar ideas.

Im not a fan of how messy our solution looks but it goes something like this:


delta = old_rotation - newRotation

sum = sum + delta


This way we can go from -3rad to 3 rad, append only -0.2 or so to the sum, and we dont care if the sum value goes over 6, it just rotates around and around.

One of the issues this causes though is if the motor lags too far behind, the error can be over 2pi witch makes the motor spin several times to catch up in this case, this is incorrect so we’ve also implemented a clamp check to make sure the error can never be more than 2pi, if it is, it just subtracts or adds 2pi to the current sum to similate a catchup, again messy but it works.

Your last

Something about this is suspicious. A swerve module should never be commanded to move more than 90 degrees (~1.5 radians) from its current angle, so you shouldn’t have errors of 2pi+.

If you were going to move a swerve module 135 degrees, you should instead move it 45 degrees the other way and reverse the drive direction. WPI’s swerve drive kinematics has an optimize method that should ensure this.

Thats exactly true but that’s higher level than the interfacing we’re talking about here. We do optimize our inputs BUT if the module lags behind, say, because the robot is turned off or low battery or some other reason. Because we have to include the summing math, the motor controller can acrew more small error buildups that never exceed 90 degrees on their own yet the motor can become more than 180 degrees behind the target.

If we could nativly set the target reference without need for our own suming math, this would not be an issue. Its not wpilib thats at fault it is the summing we need to do on our end.

Hope im explaining ok, in short, because the motor controller dosent see rotations as looping around, its all one long linear system, where 0, 2pi and 4pi are all 2pi away from each other, where in reality we know that they are 0 away, if the operator swings the controller around too fast for the wheels to keep up, they can snap a full 360 degree rotation and the motor will take the long route around because its using a linear targeting model.

This goes away if we remove the summing math, but then we have the problem of the robot swinging from -pi to pi every time the wheel approaches being pointed directly backward.

Its, just a lot of mess, sorry for the way that was worded it came off a little defensive, id like to make some cleaner code and this works well right now but, i feel like interpolation between rotational targets should be a thing on the motor controller’s side.

No offense taken - I only have the tiniest window into your problem and your code, so it’s very likely that the solutions I’m offering aren’t particularly helpful.

That being said, I think if you using something like the below code you may be able to avoid the problem of accumulating error over time. This code doesn’t persist any state - each time through it gets the current angle of the wheel and finds the “best” angle to move to for a given target angle.

 * @param targetAngleInDegrees target angle from WPI's swerve kinematics optimize method
public void setSparkAngle(double targetAngleInDegrees) {
    // Get the current angle of the motor by reading the current rotation count and multiplying it by 
    // an experimentally derived ratio
    double currentSparkAngle = spark.getEncoder().getPosition() * degreesPerMotorRotation;
    // Slide the target angle until it is close to the current angle
    double sparkRelativeTargetAngle = reboundValue(targetAngleInDegrees, currentSparkAngle);
    // Ask the spark to turn to the new angle. Also, convert from degrees into native motor units (rotations). 
    spark.getPIDController().setReference(sparkRelativeTargetAngle / degreesPerMotorRotation, ControlType.kPosition);

private double reboundValue(double value, double anchor) {
    double lowerBound = anchor - 180;
    double upperBound = anchor + 180;
    if (value < lowerBound) {
        value = upperBound
                + ((value - lowerBound) % (upperBound - lowerBound));
    } else if (value > upperBound) {
        value = lowerBound
                + ((value - upperBound) % (upperBound - lowerBound));

    return value;

Yes this is essentially exactly what we have right now. Except we called it a clamp and not a rebound value. Also we use position scaling.
And you still have the issue where you will have a snap-around? Im not sure exactly, but because you are not summing delta over time, SparkRelativeTargetAngle is bound between -180 and 180 witch means it will snap around when the wheel aproaches that limit?

I believe you are missing something in the above code. There will never be snap around, because SparkRelativeTargetAngle is not bound between [-180, 180]. It’s bound between (currentSparkAngle-180, currentSparkAngle+180].

If the spark is currently reporting that it is at 360 degrees and you give it a command to move to 45 degrees, the above code will transform from 45 degrees (targetAngleInDegrees) to 405 degrees (sparkRelativeTargetAngle), because 405 is the best representation of 45 degrees in the [180, 540] (aka [360-180, 360+180]), degree space.

I think I’m begining to see what you’re saying, but you’re appending a rotation (say, 45) to what your spark max reports, we tried this actually but our loop runs pretty quick and very rapidly you loose rotations if you trust the encoder like this, one loop may request 20+ plus to get to a ‘global’ angle of 20 degrees, and the next might want to go to 90 but the motor has only moved 2 degrees and the new target angle is 72. I think i might be missing where you account for this but the code is very familer, we ran that at week zero and the wheels got out of alignment pretty quick. Now we use the reported value for piping to an optimize function, but keep track of a global summation of angles so the target is always imagable to a (-180, 180) space.

Sorry for being slow, apart from that i’d like to spend more time with your custom reboundValue :slight_smile: i think this might help~ ill see if i can produce some tests and ill let you know if i’ve got a better understanding, thanks so much for your input.

Would sure be nice if we could just set a target angle and let the spark max interpolate~

The latter half of your statement is a little hard to parse, but for the first part - the above code does not append rotations to what the spark max reports. Instead, it is scaling an absolute target angle (e.g. you want your module pointed at 45 degrees) into a value close to where the spark max currently is.

To further clarify - this function isn’t used to apply a delta to the existing angle. It doesn’t take the current position of the module and rotate it by X degrees. If you want the module pointed at 45 degrees (e.g. targetAngleInDegrees is 45) and the spark max is currently reporting that it is at 405 degrees (currentSparkAngle returns 405), then nothing will happen (because sparkRelativeTargetAngle will also be 405 degrees).

I see now, sorry about being a bit confusing, its hectic here and I’m going through lots and lots of code. This is easier to digest with your explanation, thanks so much~ I like this implementation a lot more.

I’m yet to test it out yet, but thank you for all your help, ill reply back when i iron this out a little and test it on the bot, but i’ve integrated it in place of our previous code, and im excited to see if it works, again thanks so much, this implementation is a lot cleaner, ill link a video when its on the bot!

    def setDesiredState(self, newState):
            Updates the current desired state,
            where we want this module to now point.
            # Optimize the input command to reduce unneeded motion.
            optimizedState = kinematics.SwerveModuleState.optimize(
                newState, self.getCurrentState().angle

            # newTargetAngle is an angle in radians.
            newTargetAngle = self.reboundValue(

            # newTargetRef is a number of rotations.
            newTargetRef = newTargetAngle / (2 * math.pi)

            # Send newTargetRef to the motor controller.

            self.desiredState = newState
    def reboundValue(self, target, datum) -> float:
            Made with help from JohnGilb on Chief Delphi

            lowerDatum = datum - math.pi  # 180 behind
            upperDatum = datum + math.pi  # 180 ahead

            if target < lowerDatum:
                target = upperDatum + ((target - lowerDatum) % (upperDatum - lowerDatum))
            elif target > upperDatum:
                target = lowerDatum + ((target - upperDatum) % (upperDatum - lowerDatum))

            return target

@JohnGilb Gave it a test just now and it seemed to cause an awful lot of spinning round and round, we’re on our way to our first comp! so wish us luck, ill try and integrate your algorithm in the pits.

Best of luck to you and your team in competition!

1 Like