Team 230 - GaelHawks Swerve Code release

Welcome to the Team Gaelhawks swerve code release thread.

Team 230 Swerve Drive Development

At Team 230 we took on swerve drive development as an off season project, with the intent to
develop a swerve drive system to use for performance evaluation and potentially at some point for
use as a prototype for a competition robot. We proceeded with the following ground rules (that
seemed pretty good at the time):

1. We would use COTS swerve drive hardware
We didn’t want to simultaneously develop hardware and software solutions for swerve. The
availability of well designed reliable COTS modules made this an easy decision. We chose to
purchase the SDS Mark 4 modules, powered with Falcon motors for drive and steering.

2. We would write all out own swerve specific code, and close all feedback within the Robo
Although we are not morally opposed to utilizing high level feedback and control functionality within the Falcon’s internal motor controller, we have had great success in the past with pulling all command and control functionality into the Robo RIO to as large an extent as possible. This has allowed us to much more easily perform system ID on the hardware and develop control system architecture. We wanted to spend more time developing swerve kinematics and control strategies, vs. spending a lot of time on determining interface requirements for existing libraries (although that is also a viable strategy).

We broke the initial software development into the following areas:
1. Swerve kinematics
2. Module drive velocity control laws
3. Module steering control laws
We prototyped the original formulation in Octave, a high level language that is for the most part
compatible with the MATLAB programming language. Octave is distributed under the GNU Public
License (GPL), and allows us to very rapidly prototype control systems in a high level language that has native support for vectors, matrices, and complex numbers.
The code development proceeded from the bottom up, in the following manner:
1. Establish the ability to drive motors and read encoder positions, velocities, and angles.
2. Establish closed loop control of module drive velocity and steering angle
3. integrate the high level kinematic equations and drive the base around.

After this was complete, we had a fairly significant laundry list of functions and improvements to
implement. In roughly chronological order, we implemented the following functionality:
1. Implement automatic fwd/reverse drive direction to minimize steering angle command
2. Add steering angle compensation to drive velocity to eliminate parasitic drive during steering
3. Develop revised Falcon velocity encoder filtering to improve control bandwidth
4. Incorporate select-able field coordinate/robot coordinate drive modes
5. incorporate gyro stabilized heading control
6. Implement command model filtering, shaping, and deadband to maximize driveability

At the current time we have implemented around 350 lines of simulation code in Octave, and about 800 lines of C++ code for the prototype drive base. This whole process took us around 6 months to complete, working a couple days a week at the most.

GitHub - Team 230


Thanks for publishing this!
Can you publish the Octave simulation code as well?

Hi velcro,
First off, my apologies for not having posted this with the code. If I may, please let me pontificate a bit to explain the simulation code.

When we decided to take a crack at swerve drive, I wanted to provide the students with at least some simulation baseline to ballpark the parameters and architecture we would use to generate the swerve drive code. We intentionally created all the control architecture we used independent of the currently available swerve libraries, not because we have an issue with them, but to focus more on the physics and less on deciphering interface requirements (seemed like more fun at the time!).

My goal was to generate 3 octave prototypes that the students could use to generate the base swerve functionality: 1. a module to represent the swerve drive kinematics, 2. a module to represent the swerve drive module velocity control laws, and 3. a module to represent the swerve drive module steering control laws. These files are swerve_cmd.m, vel_claws.m, and steering_claws.m respectively. These functions I hope are sufficiently documented to be of some use.

The file robot.m is the high level routine to generate a simple motion profile and exercise the swerve kinematics and control laws. If you simply run “robot” from the Octave command line is should generate a simple trajectory with a plot for the robot motion and the velocity and steering behavior for module 1. My primary debugging technique was to generate the path of the 4 swerve modules, connect them with a line, and make sure it looks like a square (or the shape of the base) while it follows the commands. robot.m should accomplish that. FYI, limit.m is simply a hi/lo limiter function.

We are in the process of including these files in our public archive. Note that the simulation does not exactly match the C code, I did experiment with some lead compensation in the sim that’s not in the robot code. We put together a drive base that weighed about 40 pounds or so and It’s scary fast. The parameters in the code are optimized for that base however, so if this becomes our drive system for this year we will do a little re-tuning.

Lastly I wanted the say thanks to The students that spent so much time coding and testing this stuff. Turns out I’m kind of a pain you know where to put up with, i appreciate their patience.

Steve Spoldi

P.S. Sorry I didn’t post the code here, I can’t sort out how, but I’m working on it!

vel_claws.m.txt (858 Bytes)
swerve_cmd.m.txt (1.8 KB)
steering_claws.m.txt (1.2 KB)
robot.m.txt (6.1 KB)
limit.m.txt (74 Bytes)

Figured it out, please remove the .txt extension.

Steve S.

Thanks! I appreciate the time and effort, especially today.