Swerve Code Expert Help?

Hi Everyone,
We are attempting to program swerve but it is currently not working. I don’t even know where to start. We are using Neos with SDS MK3 and CTRE CanCoder sensors.

If all you can do is point me to working code of this setup, I’ll take it!

FRCTeam5484/SwerveTest (github.com)

1 Like

I don’t see anything that is too clearly wrong. What exactly isn’t working? If you just want some code to reference you can look at this code from 364.

1 Like

What isn’t working? Knowing what isn’t working helps us find the code to debug.

The wheels don’t align properly. The Y axis movement on the controller does not command the wheels forward or backward. The X axis moves the wheels very slow. The Turn controller command makes the wheels rotate endlessly.

This is all while testings in a static position. I can’t put it on the ground until the wheels align properly. I have set the offsets by manually aligning the wheels and recording their offsets.

1 Like

A very general suggestion: The angle measurements seem like very tortured calculations and are too hard for me to follow. Make sure they are doing what you intended with print statements.

And don’t forget that the gyro doesn’t move with the robot on blocks, if that matters to your angle calculations (probably shouldn’t since that is normally used just to flip to field orientation).

1 Like

So a quick look at your codebase doesn’t reveal an obvious issue. This isn’t to say there isn’t one or more, but it just means I don’t have a “silver bullet” answer for you.

General advice for how to start to break down the problem of “sweve doesn’t work”:

  1. Work from inputs to outputs
  2. Pick a small subset of functionality to check out
  3. Use things like SmartDashboard.putNumber("name of value", value); to inspect how numbers inside your codebase change over time.
  4. Think like a scientist. Make a hypothesis about how a certain piece of code should behave in a certain scenario. Add enough debug info to allow you to prove or disprove your hypothesis. Perform an experiment on your robot, capturing data. Then analyze the data to determine if your hypothesis is proven or disproven.
  5. Work methodically. Break your code, at least mentally, into chunks of “input, processing, output”. If inputs are “what you expect” (working), but outputs are “not what you expect” (broken), it would imply the processing between them is broken. If inputs are broken, move your debug efforts to the things which drive those inputs. If outputs are correct, move downstream to the next steps to find something that is broken.

Finally, the one universal piece of advice I always ask everyone doing SDS modules to check before they get too deep into software:


OK, lets start with clarifying questions. Does Swerve Constructor need 0-360 or -180 to 180? What methods require that?

Is the offset value the actual reading when the wheels are aligned correctly?

1 Like

I’m not sure I have found your problem, but a few observations.

In SwerveModule, you have:
config.sensorCoefficient = 2 * Math.PI / 4096.0;
and then later:
angle *= 2.0 * Math.PI;
Seems to me like you don’t need the second one.

In cmdDrive_TeleOp you say:
ySpeed = Math.abs(xSpeed) > OIConstants.kDeadband ? ySpeed : 0.0;
That should be ySpeed.

In SwerveSubsystem, when you call com.kauailabs.navx.frc.AHRS.getAngle(), I believe you need to negate the angle because this API returns clockwise yaw, and FRC/WPILIB generally uses a counterclockwise convention.


I fixed those items and I’m trying these tomorrow. THANK YOU!!!

1 Like

How did that work out for you?

I just wanted to add that, for deadband, you could consider using MathUtil.applyDeadband.

Status Update. Swerve is working and it required basically re-writing everything and some code ducktape. Feel free to use if you want it. On a side note, I think I understand it now.

FRCTeam5484/SwerveTest (github.com)

1 Like

I’m glad you made a successful swerve drive. I’m surprised it drives better with your change from brake to coast motor.


I changed it back to kBrake. I may have not pushed that back up. I put it in coast temporarily so I can manual turn the wheels to get encoder values.

Consider keeping your drive train in brake mode while enabled and switching to coast mode a few seconds after it is disabled. I wrote up some notes on how to do this.

1 Like

Nice! Thank you for sharing!

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