Software Update #11: Learning to Crabwalk
No crab is complete without its code. Or something like that. We’ve been working on swerve code since early August as part of our offseason training for software, which means that a good chunk of this code was created by some of our newest students. Thank you to everyone on 6328 who dedicated their time to this project!
The Basics
All of the code is in our SwerveDevelopment repository on GitHub (the drive subsystem is here). The code structure closely resembles our 2022 competition code, including full integration with AdvantageKit. This means that the hardware interface for each subsystem is separated from the control logic, allowing us to accurately replay matches using the WPILib simulator or seamless switch to a physics simulation (more on that below).
The swerve support uses WPILib’s built-in kinematics for most functions with feedforward/feedback controllers running on the RIO. We chose to bypass the onboard controllers on the SparkMaxs for a variety of reasons — this simplifies the code running the controllers (no need to artificially wrap turn setpoints), it makes it easier to run with simulated modules, and we can use our new derived velocity controller for the drive motors (see the section below for details on that). We haven’t observed any performance issues with running the controllers on the RIO.
Odometry is one area where we deviate slightly from WPILib’s built-in kinematics. The supported approach for 2022 has been to update odometry based on the velocities of the modules (running the kinematics in the reverse direction of when driving the robot). Instead, we’re using the position delta of each wheel, which reduces the amount of drift over extended periods. Our odometry code is here — we just use the kinematics object but feed it a position delta instead of a velocity, then modify our pose using a Twist2d. I believe WPILib is planning to make position delta odometry the default for next year.
Enough babbling! Here’s a video of the code in action, comparing a video to the odometry and module states. The red arrows show the measured states and the blue arrows show the setpoints.
Generating a feedforward model and tuning the controllers is especially important for swerve, and we have a couple of tools we’ve developed to help with that:
- We rely heavily on our
TunableNumberclass for values like FF and PID gains. These values act like constants when running on the field, but can be manipulated using NetworkTables when the robot is running in tuning mode. - Last year, we wrote a
FeedForwardCharacterizationcommand that generates values for kS and kV using a the same technique as SysId’s quasistatic tests. Integrating this function into our code allows us to recharacterize quickly without needing to configure SysId and deploy new code. It also means… - We can run the characterization routine on the drive motors by just controlling the turn motors to zero degrees. No need to use blocks or rely on brake mode for the turning motors. Our drive subsystem has a characterization mode to support this use case.
Trajectory Following
Our preference has always been to define trajectories in code using transformations (rather than relying on a GUI tool). Last year, we created a FieldConstants class that defined lots of useful reference points on the field:
All of our trajectory waypoints were defined relative to these reference points (example). Defining these waypoints in code also allowed us to extensively reuse waypoints rather than redefine full trajectories for increasingly complex autos. You can see that in action with our five cargo auto, which uses waypoints from our two, three, and four cargo autos (code here).
We wanted to continue defining trajectories in code for swerve, but WPILib’s tools don’t provide a complete solution for controlling the holonomic rotation along a path. We created our own set of trajectory classes for swerve to address this issue, which can be found here. Let’s look at an example trajectory definition:
CustomTrajectoryGenerator.generate(config, List.of(
new Waypoint(new Translation2d(0.0, 0.0), null, Rotation2d.fromDegrees(90.0)),
new Waypoint(new Translation2d(2.0, 3.0), Rotation2d.fromDegrees(90.0), Rotation2d.fromDegrees(-90.0)),
new Waypoint(new Translation2d(2.0, 6.0), null, null)
))
Each waypoint has three components defined in the constructor — a translation (required), a drive/velocity rotation (optional), and a holonomic rotation (optional). Since both rotations are optional at every waypoint, the generator will automatically combine quintic and cubic splines for the drive path, then use an S curve for the holonomic rotation. We also have shortcuts for generating waypoints based on poses. For example, moving from one pose to another on swerve (following a straight line) looks like this:
CustomTrajectoryGenerator.generate(config, List.of(
Waypoint.fromHolonomicPose(startPose),
Waypoint.fromHolonomicPose(endPose)
))
Our CustomHolonomicDriveController can follow these generated trajectories, including correctly handling the holonomic rotation feedforward. Here are some key examples from our swerve code:
- Setting up the config and generating the trajectory (here)
- Running the drive controller (here)
- An auto routine using our trajectory following command (here)
You can see the trajectory follower in action at the end of this video:
Simulation
Last year we relied very heavily on simulation to test drive code, auto routines, driver assist features, and more. We knew we wanted to support this on swerve, but creating a full swerve simulation becomes complicated very quickly. Instead, we opted for the simpler approach of independently simulating each drive and turn motor using the FlywheelSim class. With the hardware interaction separated from the drive subsystem, enabling the simulator is as simple as swapping our ModuleIOSparkMAX implementation for ModuleIOSim.
Another problem we needed to solve was simulating the gyro sensor. The angular velocity of the robot can be calculated using the measured module states (the outputs of the module sims) and applied to the pose, but this approach is suboptimal on the real robot. In that case, we want to rely on the gyro because it’s more accurate over long periods. We set up the odometry system to automatically switch between these two approaches depending on whether a gyro is connected (code here). The sim simply doesn’t provide a gyro hardware implementation, meaning the “disconnected” case is used. A nice side effect is that we have a fallback if the gyro is ever disconnected on the real robot.
This system works very well to check that the drive and turn controllers are behaving correctly. It can even approximate some more “complex” behaviors like realistic acceleration (making this work mostly involved adjusting the flywheel sim parameters until the results looked reasonable). Ultimately, we feel that this approach achieves everything we require of the simulator despite not being physically accurate.
Here’s a video of a five cargo auto running in the simulator:
Derived Velocity
While we have generally found great success with the SparkMax/NEO ecosystem, the fixed velocity filtering on the internal encoders has been a distinct low point. For those unaware, the SparkMax filters the velocity measurements from the internal encoders, resulting in a ~100ms delay in readings. This makes it more difficult to tune aggressive feedback controllers. While we’ve been able to work around this with well calibrated feed forward models, eliminating the latency would make it much easier to tune the velocity controllers for mechanisms like flywheels and drivetrains. We created a class that replaces the built-in PID controller on the SparkMax and eliminates this measurement latency:
You can find the code here: SparkMaxDerivedVelocityController
This works by running the controller on the RIO in a notifier thread while calculating the velocity based on the SparkMax’s position measurements (which are not filtered). Running that calculation requires accurately timestamping the measurements from the SparkMax (data which is not provided by REVLib), so we manually read the CAN frame for position from the SparkMax. Using a notifier allows us to run the controller faster than the loop rate of the main robot code, though for most applications a 20ms period appears to be sufficient. You can also manually adjust the level of filtering on the data, since completely unfiltered velocity data can be extremely noisy.
Here’s an example of velocity data from our swerve drive with the SparkMax’s measurements in yellow and our derived ones in blue:
We also tested this system on the flywheel for our 2022 robot. We shot two balls in succession after raising the kP value until just before it oscillated at steady state. Here’s what the internal (filtered) controller on the SparkMax did:
After the first ball, it overshoots the setpoint of 3000 RPM by several hundred RPM (blue is our derived value). The second ball is then fired with the flywheel running too fast, which could affect its trajectory. The controller overshot the setpoint because the data showing that flywheel had reached the setpoint was delayed, and so it continued to apply voltage after it should have stopped. In contrast, here’s the same test with the controller running on the RIO using our derived value:
The flywheel speed recovers perfectly after the first shot and is stable at 3000 RPM, because the controller is able to respond more quickly. You can find the implementation of this controller for our flywheel here, and (as shown before) we’re also applying it to the drive velocity controllers on our swerve bot.
Feel free to reach out with any questions, and keep an eye out for more software updates soon.
-Jonah
