Help With WPI Field Relative Swerve Issues

Hello everyone,

My team and I have been working for a while now to build the team’s first swerve drive. I was the main author of a large portion of the code, so I am responsible when something inevitably goes wrong. In preparation for the competition and driving, we wanted to utilize field-oriented (also called field relative, I’ll abbreviate to FOD). We have used all of the WPI framework for the math with minimal issues so far. However, when trying out the field relative driving mode, we ran into some issues.

When driving in robot relative mode, we don’t have any issues. The robot responds as you would expect.

However, when driving in FOD mode, the robot doesn’t seem to obey the X axis when it comes to moving (forward / backward). When moving left and right, it seems to work just fine. When the robot is facing you or facing away it works perfectly. However, as soon as you begin to turn, it doesn’t work. I’ve tried to find a solution with no luck. Any help would be greatly appreciated. Here is the link to our Github repo.

Thanks,
Christian Piper
Lead Programmer, Team 834, Spartechs

2 Likes

Have you tried calculating the conversion to field oriented yourself? If you tried it yourself you would know if the error is in coming up with the chassis speeds or not.

What you do is convert the X and Y components of the desired field oriented speeds into an angle and magnitude, rotate this by the gyro orientation, and convert back to X and Y components of velocity.

If this has the same problem you know that the error isn’t actually with the field relative part of the code.

I see you’re using the getRotation2d() method on your navx. I haven’t used a navX (we use a pigeon), but looking at the source for the AHRS class, and subsequently the source for the Gyro class, I don’t think that’s the method you want. I would suggest using getFusedHeading instead.

getRotation2d will only return the yaw (z-axis), which technically is the correct axis if you’ve mounted your navx horizontally. If you need to use a Rotation2d object, just pass in the fused heading like so

Rotation2d.fromDegrees(navx.getFusedHeading());

1 Like

+1 to @GregBilletdeaux. Assuming that both SetModularStates and WPI’s ChassisSpeeds.fromRelativeSpeeds functions are working correctly, then that seems to be the one thing that might be wonky.

A Small Side Note
  public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {

// Set up the modules
if (fieldRelative) {
  setModuleStates(ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, Robot.navX.getRotation2d()));
}
else {
  setModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot));
}

}

This method was really freak’n beautiful to read. It follows SRP, it reads nicely, it’s short. Major props, I don’t get to see code like this often.

2 Likes

Thanks for the compliment! I will definitely try the Rotation2d.fromDegrees(navx.getFusedHeading()); suggestion by @GregBilletdeaux. Our next practice is tomorrow, Tuesday (10/26), so I’ll let you know how it works after that. Thanks for your help!

1 Like

If that doesn’t work, you can try what my team did; we also have a NavX!

Before continuing, I want to mention that we did not use any helper classes from WPI; although, I see you did. Therefore, our implementation is somewhat different. I see that you seem to have some Kinematics methods that do these calculations, but if you still can’t get it, try switching it up! My implementation of field-orientation should work since it just changes the raw joystick input.

My approach was to first get the angle to 0, which I do below (note that it is in radians, despite the comment!)

theta = self.getAngleTo(0) * (
            math.pi / 180
        )  # Gets the offset to zero, -180 to 180.

This zero should be which ever way you want to be forward on the joystick by the way.

Next, we apply the manipulations to the X input (typically left to right on a joystick, the strafing) and the Y input (forwards and back, towards and away from the driver). Here is our code:

 temp = y * math.cos(theta) + x * math.sin(
                theta
            )  # just the new y value being temporarily stored.
            x = -y * math.sin(theta) + x * math.cos(theta)
            y = temp

If you haven’t caught on yet, we use Python! Regardless, it should still work. Just recall that radians are required in both of our languages! Although the math is confuzzling, it should work! I would try to explain it with vectors and fun stuff, but I am in the middle of class right now!

However you fix it, I hope you get it working! Always happy to help another FMA team!

3 Likes

That’s exactly what I did…far simpler than messing with the drive code itself IMO.

1 Like

Have you confirmed that the positive rotation direction of the NavX corresponds to the positive rotation direction assumed by WPILib?

1 Like

Per A question about the first generation of navx - #3 by JamesT to use getFusedHeading the magnetometer should be calibrated. And per the linked magnetometer calibration, it’s not necessary for most applications including field oriented drive.

What rotation convention does WPILib use? Positive rotations are CCW?

2 Likes

Note that the warning admonition is incorrect on that page. Joysticks still follow NED convention. Joystick axis values (e.g., GetY()) are rotations around the given axis. When viewed with each axis pointing toward you, CCW is a positive value and CW is a negative value.

Pushing forward on the joystick is a CW rotation around the Y axis, so you get a negative value. Pushing to the right is a CCW rotation around the X axis, so you get a positive value.

Also, NED convention only applies to the drive classes, not the whole library. Everything else uses North-West-Up because that’s what math uses. Aviation uses NED, so they’re weird. :stuck_out_tongue:

1 Like

Oceaography uses NED as well. Down actually makes more sense for oceanography. I think geologists use NED, too.

That makes sense for those because you reference everything from the ground or sea level. In aviation, they decided to reference everything from the aircraft frame.

“Where am I at on the earth?” versus “Where is the earth at compared to me?”

Some people just gotta make it all about themselves :stuck_out_tongue:

2 Likes

Hello everyone! It turned out that the NavX that we were using was bad and reading angles all over the place. We dropped in another NavX tonight and field oriented worked perfectly. Thanks for everyone’s help! Have a nice day / night!

2 Likes

Glad you got it working! My team got metal chips in the MXP once and that made us think our NavX was broken once!

Metal chips probably got into the NavX… we had a bunch of work to get done for Ramp Riot and the timing got a bit tight for continuously removing components and putting them back on.

3 Likes

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