Can anyone review this swerve code

The code below is in place to allow for the forward and strafe functions of the swerve modules. I wish to know if we have done something wrong with this code because our team has found that this is more than likely the source of the issues we have been having.

final double temp = forward * Math.cos(angle) + strafe * Math.sin(angle);
strafe = strafe * Math.cos(angle) - forward * Math.sin(angle);
forward = temp;

angle in our java code is our gyro the ADIS16448_IMU z axis and it returns angles that seem to be correct only drifting very sightly over 5 seconds or so. This rules out that the gyro is giving the wrong values and so we need to know if it is the equation we have done wrong.

So if the angle inputs are correct then it doesn’t make sense to me why our robot switches the forward direction every time we turn more than 90 degrees. This is not how field orientated drive is meant to function. The code above is the only code that directly could effect this and I was wondering if the way we set the equation up is the issue or if there is another outside source for the problem.

Thanks for any assistance. Team 5173 the Robohawks from Fennville. Michigan.

For field oriented control, we simply subtract the gyro angle from the commanded stick angle before plugging it into our robot-centric drive code. I’m not exactly sure what your code is doing, what “temp” or “angle” refers to, or what “forward” is. Linking your Github may be wise.

You can have swerve code that uses a simple vector instead of a joystick angle. It looks like they’re rotating it 90 degrees and rotating it based on the gyro offset at the same time. I’m not going to confirm right now if that is correct because I dislike rotation math and usually try to abstract it away from my swerve code.

I don’t know what this code is doing:

// wheel azimuth
wa[0] = Math.atan2(b, d) * 0.5 / Math.PI;
wa[1] = Math.atan2(b, c) * 0.5 / Math.PI;
wa[2] = Math.atan2(a, d) * 0.5 / Math.PI;
wa[3] = Math.atan2(a, c) * 0.5 / Math.PI;

Did you mean to multiply by 180 instead of .5 to convert the angles to degrees?

Also, which wheels do wa[0] through wa[3] correspond to?

I am going to try multiplying by 180 however I do believe that is fine as .5 because that portion seems to work as intended however maybe there is something im missing with that portion of the code. Also our wheel arrays are as follows 0 = left front module, 1 = right front module, 2 = back left module, 3 = back right module.

What is happening? It’s not entirely clear. When you rotate the robot it driving in the wrong direction? At exactly 90 degrees? The multiply by .5 is correct.

You’re using a lot of hedge words - sometimes these are appropriate on the internet to indicate approximate knowledge. Rarely is an approximate knowledge of functionality sufficient while determining if code will work.

OP, Prove it is correct.

A simple question to kick-start that proof: What units does your code assume the wa[] array is in?

I would also agree, a more specific description of the symptom is required. The code has too much content for me to look at it and imagine how it will run.

From what I’ve gleaned: You are operating a robot with intent to do field-oriented control. The robot’s wheel modules are acting in a reasonable fashion, but the reference for “downfield” is inverting based on where the front of the robot is facing.

You claim that the gyro reported angle is reading correctly. Specifically, how have you evaluated this to be true? I am in particular concerned of which variable(s) values were used. And, how does it relate to the final value of angle, as calculated at this line.

If you’re using WPILib 2020, the ChassisSpeeds object has a static method called fromFieldRelativeSpeeds that can be used to return forward, strafe and turn speeds for your swerve drive and fed into the new SwerveDriveKinematics class to compute wheel angles and speeds.

Your talons are named such that your wheels are 0,1,2,3 = LF,RF,LB,RB but you create your Wheels array in a different order. I’m not sure if this would cause the specific bug that you are seeing or not.

wheels = new Wheel[]{

  LBWheel, RBWheel, LFWheel, RFWheel


Also, in you can see that the 0.5 that you are multiplying the wheel azimuths by is the rotation of the swerve module. This number is then multiplied by the encoder ticks to get the azimuth position. See line 74 in

* @param azimuth -0.5 to 0.5 rotations, measured clockwise with zero being the wheel's zeroed
*     position

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