Official SDS MK3 & MK4 Code

Jacob @Kaboomboom3 and I are excited to release Java code for the Swerve Drive Specialties MK3 and MK4 modules.

swerve-template
Basic code to get a drivetrain up and running with minimal code changes
GitHub: https://github.com/SwerveDriveSpecialties/swerve-template

swerve-lib vendordep
GitHub: https://github.com/SwerveDriveSpecialties/swerve-lib

Features:

  • SDS Mk3 & Mk4 module support
  • Support for NEOs and Falcon 500s
  • Easy to use helper functions to quickly get a new drivetrain up and running
  • No PID tuning required
  • Example robots

Please let us know if you have any questions and we will do our best to answer.

26 Likes

Good stuff!

Are these values that 2910 has tuned / tested to work? Do you recommend teams retune PID loops for their modules or are these values good enough?

1 Like

Jacob has put in the work to determine good PID values for steering for both NEOs and Falcon 500s. The coefficients in the code should be good to use without any need to tune for a specific robot. With this code the steering PID is being done onboard the motor controllers. We have found that this enables very tightly controlled PID loops which result in snappy steering response.

7 Likes

Providing the sample code with all the variations and associated PID values is awesome, thanks for the extra effort!

Is there any plan to add a simple autonomous sample alongside this code? This seems like the last significant barrier to SDS being a truly plug+play swerve system for almost any team.

4 Likes

Yes and… a Swerve edition ROMI. :wink:

5 Likes

Just tried this yesterday and will say the setup process went very smoothly! Although I ran into a big issue, the bot works fine on a block and the turn motors are fine(albeit a bit jumpy) but when on the field and driving it has a considerable amount of drift.

Any ideas?

1 Like

Thanks for this very good source . However , l am wondering whether there is any instruction on how to use these code. It seems that it is not so friendly to dig into the code itself . Something like a white paper may help rookies to learn better!

Drift is usually gyro associated. I’d try and plot your gyro angle to the SmartDashboard and watch it while you just let the robot sit for a couple minutes. Make sure it’s not drifting wildly at a rapid rate - i.e. it should be relatively stable for the length of a match. If you were playing with it for 30 minutes without changing batteries or restarting the robot, I’d expect some drift.

(Also makes sense that it’s a little crazy on blocks. Friction plays a big part in PID tuning)

Given how widely this code is likely to be used, and how much derivative work is likely to come from this code, it would be good to explore using ProfiledPIDController, and/or the equivalent on whichever motor controller (especially for steering).

The basic idea is that these controllers work to drive an error to zero. Take the steering controller – its job is to ensure the given swerve module is oriented as commanded. Control inputs can cause the error term to jump (step change in setpoint ==> jump in error). The steering motor moving the module causes the error to change. The controller is chasing zero error.

A basic PID (P/PD/PI) controller uses the position error, and the time integral and derivative to try to smoothly land the module at zero error, at the desired position.

ProfiledPIDController defines the PID error in a different way – it essentially represents planning ahead to arrive at zero position error, then uses PID to deal with the deviation from this time-based plan; there PID error is the position error from the plan, not from the setpoint. This means that PID has less to do, the motion profile is essentially a form of (dynamic) feedforward. In this context, this should make the tuning parameters more robust (and also should allow even better performance). In particular, the plan part prevents the PID part from seeing step errors, and errors should generally be much smaller (unless they accumulate because the feedforward is way off). These things mean PID can be tuned more aggressively, and should make the tuning sweet spot much larger.

The key parameters for this type of motion profiling are the maximum velocity (rotations/s for steering) and acceleration (rotations/s² for steering). There’s a good overview here. WPILib has done a bunch of work in this area.

Any plans to look into this?

3 Likes

I would recommend looking into the calibration procedure for the IMU that you are using. We have been using the CTRE Pigeon most recently and have found that the temperature calibration is a very important step to take to for the least amount of drift.

It is also good practice to let the robot sit still for a bit after the IMU is powered up. Before a match when we are placing the robot on the field we always only power up the robot after it is in the perfect starting position and has been loaded with game pieces.

1 Like

Thanks for the reply. Definentally will try that tonight.

Presumably, all the robots using this code will have the same mass…?

1 Like

That might be a fair assumption given a lot of robots are right at the weight limit.

1 Like

The default PID tuning for steering will work pretty well from everything from tiny mini swerve robots to full weight competition robots.

This swerve code does not currently handle path following. If we were to add path following robot mass would become important. It has a large effect on the characterization, most notably the acceleration feed forward coefficient and path following PID parameters.

This is certain something we could look into. I believe both the Talon fx and Spark Max support trapezoidal motion profiling. We are actually really pleased with the current steering response with only PID to the target set point so I expect the improvement to moving to motion profiling will be pretty minimal.

3 Likes

One thing I want to clarify is that teams can do autonomous with this template code. They will just need to hook up the WPILib odometry and set up a WPILib path follower.

We (2910) have been using the SDS base code on our 2021 robot Baby Grond with our own implementation of path generation/following. It has worked great. While it is highly featured our (2910’s) version of path generation/follow is not nearly as polished as the WPILib version with a nice user interface and detailed documentation etc… so Jacob and I chose not to include it the official SDS code release.

Adding any kind of path following is very sensitive to robot mass, cg, etc. So everyone who uses it will have to go through the characterization process.

7 Likes

Just curious, what was your process for tuning them? And what was the robot specs (weight, wheel offsets, etc.) that you used for these default values?

The process used for tuning the steering is same process 2910 uses to tune their modules:

  1. Turn the robot on its side or upside down so the modules are free to rotate
  2. Setup the robot code so that the steering reference angle will change by 90 degrees at some fixed rate (I believe I set it to change every second)
  3. Tune the PID constants to get a good response.

We’ve never noticed that weight on the modules from the robot causing a noticeable impact on the robot. In fact, we have always tuned each configuration of modules once and used those same constants on every robot that used those modules. For example, our 2017 mini swerve used the same tuning as our 2018 robot (which was at the maximum weight).

Wheel offsets don’t have any effect on the steering tuning.

6 Likes

Thanks to Jacob, Patrick, and everyone else who contributed to this amazing resource!

This came out just as we finished the electronics on our practice mini-swerve and the code worked right away. Additionally, the code is written well and was really easy for our software team to understand.

The Spartabots are really happy with our Swerve Modules and their code, and we are excited to use them on our 2022 robot!

3 Likes

A heads up for other teams - change your CANCoder settings, we spent about 2 hours working out code errors when it turns out you just need to use PhoenixTuner config tab to do the following:

  • Change the direction from “false” to “true”
  • Change the bootup behavior from booting to zero to booting to absolute value

Otherwise, just make sure to set your offsets to 0 before your measure - I didn’t read the readme thoroughly enough and spent 20 minutes fumbling with measurements with offsets already set.

That said, the template works incredibly well and I’d like to extend a massive thanks to SDS for providing such consistent and easy-to-use example code!

4 Likes

Funny you should mention this. I have been lurking in these swerve threads for a while now, and I have to admit that my team and I are intrigued.

While not a Romi (though you could theoretically control one with a Romi Chassis without the built-in wheels), you can make a pretty portable Rio-powered vexbot. We have a plethora of old Vex hardware hanging around.

As such, I think for us, the entry point into Swerve is going to be these.

That would let us know if we have the programming chops to effectively implement swerve.

I should probably mention that going this route may be annoying as the vex motor with encoders we have are I2c. We do have many external Vex shaft encoders though.

2 Likes