Hi! Over the fall and into build season, we’ve been working on developing code for a swerve base using SDS MK3 or MK4 modules. In particular, we’ve attempted to maximize debugging ability, optimize controls, and use WPILib’s built-in classes wherever possible over vendor libraries or custom code. The codebase works for both teleoperated control as well as autonomous paths using PathPlanner. We don’t expect other teams to use this codebase, but wanted to share it as a resource that will hopefully help a bit with the complicated task of programming swerve.
The code is accessible in our frc-2022 repo, and is on the main branch (to access just the drivetrain code later in the season, go to the feat/drivetrain branch). https://github.com/team2485/frc-2022
Major challenges along the way:
Working with CTRE’s weird units (pulses per 100ms) along with normal units
Configuring encoder offsets and ranges
Varying robot heading while on a path (this required creating a slightly-edited custom version of SwerveControllerCommand)
Controls implementations:
Feedforward + velocity control on drive motors
Motion-profiled position control on turning motors (using feedforward as well)
Goals for the future:
Turn-about wheel functionality in addition to turn-about center
Polar mode control — instead of x and y, control radius and angle around a centerpoint (ie the hub)
Note: The codebase uses our team’s shared library, WarlordsLib, for a few controller functions (basically, rewriting the XboxController class to play nicer with command-based, and deadbanding), as well the IDManager class for dynamically switching between constants on different robots. However, these are not necessary or recommended for swervebase code (they are useful for our team, but YMMV).
Thank yous: thanks to @P33ge for making his code for a similar swerve base public and answering a ton of my questions! Thanks to @vargoose84 for doing the same for swerve with Pathplanner. Thanks to @mjansen4857 for creating PathPlanner, an easy-to-use and very well-implemented path creator for swerve and non-swerve drives. And finally, thanks to the WPILib devs for constantly improving their library and being very helpful with any and all questions.
Polar mode control is an interesting challenge, methinks you need spot on odometry throughout the match or constant ‘rezeroing’ in order to account for the hustling and slipping that’s gonna affect your motors as well as your gyro.
Let us know if you figure out an "easy button " way
Yup, gotta set the challenge to meet it! I think using Limelight vision input as well as WPILib’s pose estimator Kalman filters might help with that. It could be pretty helpful for drive though even if it’s not absolutely perfect.
We found that only kP was necessary for us (i.e. kD wasn’t helpful, it caused some oscillation). We baseline estimated kP by finding the maximum error we could have for drive, which was basically the maximum speed of the drivetrain (4 m/s) in pulses per 100ms, which is about 21,000. If we want maximum output there, we would have a kP of 1023 (the maximum output in CTRE units) divided by 21,000, about 1/20, or 0.05. From there, we tuned it a little as well.
Motion magic is essentially just a trapezoidal motion profile. It allows us to have a consistent acceleration and velocity throughout position control, which improves the smoothness and consistency of turning.
SDSpublishes the free speed of their swerve modules, which is what I based the estimate of 4 m/s on.
No, our drive gear ratio is 8.16. (The calculations are in the Constants file).
1023 is what CTRE uses in its PID loops as “full output,” so yeah, if you’re controlling onboard the motors this would be the maximum output.
This one’s kind of arbitrary. I started it low and kept increasing it until I was happy with the performance. I might have done similar calculations, though, so I’ll update this once I’m back in the shop and can look at the math.
I also forgot to respond on voltage compensation, basically this adjusts output voltage so it’s consistent across different battery voltages.
Thanks that, l see that you have used kp ,kd and kf in your turning motor, how do you calculate these three parameters? And if l want to test which kModuleMaxSpeedTurningRadiansPerSecond fits l best, l will start from 1 Math.pi and increase until l am happy with the performance. But what is the best performance, is there any creteria for it?
Let’s start with kF. There’s a guide on the CTRE docs for finding kF, we did so with a regular output of 40% (this was somewhat arbitrary to be honest). For kP, we basically did the same thing we did for the drive motor. Maximum error with optimized swerve headings is pi/4 radians, which is about 3000 pulses. To get 1023 (full output) kP would be about 0.3. We tuned it down to 0.2, though. We introduced kD when we had a bit of overshoot, and kinda tuned it arbitrarily. Not sure if there’s a great way for benchmarking kD, if anyone more versed than me wants to chime in that would be cool.
That looks pretty good (you’re getting between setpoints fast), but it’s hard to judge because I don’t know exactly what you’re doing and I haven’t extensively used Phoenix for tuning. It looks like you don’t have motion profiling on, or that the velocity limit is a little too high. You’re ultimately the judge of performance, though.
As @vargoose84 mentioned above, you will need accurate odometry throughout the match for this because you will need the center of rotation (e.g. hub) relative to the robot center.
Oh right!!! That’s pretty sweet. It’s kind of different than what I had in mind (basically your x and y would be replaced by radius and angle) but still works. Another goal is to use the dpad to let drivers turn about any wheel.
Motion magic is CTRE’s implementation of a trapezoidal motion profile.
Basically the normal strafe movement (left x stick) would rotate the robot around the hub, and the normal forward/back movement would get closer or farther from the hub.
Ether’s papers from 11 years ago derive most of everything you want to know about swerve, including what he coined as the moon, rotary and dosado maneuvers.
