Omni-directional "Swerve" Programming
Overview
Over the summer of 2022, I had to develop code for an omni-directional robot drivebase geared towards the First Tech Challenge Competition. I did this project mainly to push what people thought was possible, and ended up creating a really innovative proof-of-concept. This page deals with the programming aspect of this project, see here for the mechanical part.
As this was a season-independent robot, my role on this project was to do all the heavy lifting as I was paving a new path. However, I also taught multiple others how to use a swerve drivetrain, and as of now still provide assistance if needed.
V1 Robot Github code repository here
Kinematics
The mechanical side of a swerve is hard, but the kinematics are harder. Luckily there was a great paper found here that helped make the kinematics easier to understand. For the first iteration, however, we used the library RoadRunner to make the initial version of the code, which came with some functional, albeit clunky, kinematics. We made some changes, however, to allow the modules to wrap-around from 0 to 360 degrees, which meant the swerve drove smoothly regardless of direction. We also made it such that the motor direction could flip so that the module would not have to spin 180 degrees to drive the other way.
Smooth Robot Driving
Kinematics derivation from a paper
Code I wrote to handle motor flipping and wraparound. I made it easy to disable so that we could find out what was causing other bugs in the program.
Sensors
In order to know the state of the robot at all times, we added sensors to every output possible. We know the heading of the robot through an IMU (Internal Measurement Unit) internal to the control board. We can know the orientation of the individual modules through absolute analog encoders, which can give information to within a degree. Finally, we have quadrature encoders built-in to every motor, which lets us know how far we have traveled in a given direction.
With this information, in conjunction with the inverse kinematics derived in the previous part, we can derive the position of the robot on the field without any external reference like a camera or distance sensors. This enables us to perform complex maneuvers like those shown on the right.
Fast, consistent autonomous routines
Early iteration of this concept
Servo issues
The biggest problem we had with this robot was actually the servos that controlled the module rotation. When they started, they would draw so much current that it would overdraw the 6v converter that powered all of the servos. We tried to address this in code using servo profiling.
Servo profiling is useful because it guesses the servo's current state based on past values and time, and can limit factors such as acceleration and jerk on the output. We tried to limit jerk in particular because that directly affects the change in current draw, which we felt was the main issue. However, after implementing the profiling, we found out that the servos we were using did not update fast enough to make full use of the profiling.
As a result, we resorted to the last-ditch hardware solution of adding more capacitance to the servos. We went overkill and did the biggest capacitor we could fit, and talked to the servo supplier to address this problem in the future.
Servo profiling code
The capacitor we ended up adding
Before and after of the capacitors