Programming Wheel Angles in Swerve drive

Technical Programming
1

So we have been working on some swerve drive code and we have come really close to achieving it. Right now however, we have one problem we are not sure how to tackle. We did all of our calculations using this paper we found and after all the calculations the angle of each wheel is in the range of -180 to 180 degrees. The basic overview of how the code works is that we calculate velocities and angles after running calculations using joystick inputs and then command each individual motor its respective wheel velocity/angle. The problem comes when we try to change the angle from say, 170 degrees to -170 degrees, which in reality is a 20 degree angle difference, the motor does almost a whole turn of 340 in the opposite direction to get to that angle.

I was wondering if anyone else that has done swerve has a solution for this problem. Thanks in advance!

2

While 170 to -170 deg is only 20deg, can your swerve handle multiple turns of the wheel? If so, you have to program your system to “look both ways” and take the efficient route.

If your system can’t handle spinning the wheel around multiple times, a faster option would be to rotate the wheel to the opposite angle of your -170deg target (10deg, for a -160deg travel) and invert the motor output to move backwards, again taking in the “look both ways approach”.

3

Whoops, forgot the important bit.

If your trig and algorithms from the whitepaper give you the proper target angles in a format you can understand, then its not the algorithms that need to be modified.

You just need to take your new “target” angle, and your “current” angle and write in your own layer that finds the most efficient way to get from one to the other. That is what my previous recommendations are relevant to.

4

Yep, we kind of already worked a little on that part and we got the logic down to change the direction of the wheel and always go for the shorter angle so we don’t do any turns that are greater than 90 degrees to keep it as snappy as possible. I should correct that what our robot is doing right now if we command the change form 170 to -170 it goes to 10 degrees and changes the direction of the wheel, which is still not very optimal. So we are trying to figure out the logic to handle the multiple wheel turns that you mentioned which we hadn’t thought about.

5

Another alternative solution here is to switch to the inbuilt new swerve libraries for 2020 in WPILib. They are tested and have kinematics, odometry, trajectory generation, and even a path follower built-in for swerve robots. You can check simple example projects through the links below.

Simple Swerve Bot

Swerve Controller

1 Like
6

What would happen with your code if you did try to go to “190deg”?

7

Thanks, Ill check those out.

1 Like
8

So i calculate the difference between the commanded angle and the current angle read from the encoder and if it is greater than 90 degrees, I add 180 degrees to my calculated direction and normalize the angle so that it falls between -180 degrees and 180 degrees as well as reversing the magnitude of the velocity.

For the -170 to 170 case, it would calculate the difference and see that it is 340 degrees which is greater than 90. It then would add 180 degrees to the 170 which will be 350 degrees which is normalized to -10 to fit the range and spin the wheel backward.

9

Here’s how we do it:

    final double speedMultiplier;

    final int wrap = getCountsPerRevolution(); // in encoder counts
    final int current = steer.getSelectedSensorPosition(Constants.PID_INDEX);
    final int desired = (int) Math.round(targetPositionDegrees * wrap / 360.0); // in encoder counts

    if(quickReverseAllowed){
        final int newPosition = (int) MathUtil.minChange(desired, current, wrap / 2.0) + current;
        if(MathUtil.minDistance(newPosition, desired, wrap) < .001){ // check if equal
            speedMultiplier = 1;
        } else {
            speedMultiplier = -1;
        }
        steer.set(ControlMode.Position, newPosition);
    } else {
        speedMultiplier = 1;
        final int newPosition = (int) MathUtil.minChange(desired, current, wrap) + current;
        steer.set(ControlMode.Position, newPosition);
    }

minChange is basically this:

double minChange(double a, double b, double wrap) {
    return Math.IEEEremainder(a - b, wrap);
}

and minDistance is just the absolute value of minChange.

The speedMultiplier and quickReverseAllowed is a bonus. This makes it so the wheel only ever has to spin a maximum of 90 degrees to get to its desired angle.

getCountsPerRevolution() is just defined as how ever many encoder counts one revolution is. If you wanted to, you could do everything in degrees or radians.

1 Like
10

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.