Swerve 2021 Pt. 3: Video and Firmware/Comms

My last post was getting a bit long due to the pictures so I decided to put the video of it working and the description of the communication/firmware in a separate post.

Video:

Control Scheme:

I have 4 independently controlled coaxial swerve drive modules. This means the motors are not on the pivoting portion of the modules and so the module can pivot infinitely while still driving the wheel.

Each module consists of one of my custom 2-motor brushed controllers, an AS5055A magnetic encoder for position control, and an Arduino Nano which handles the module control. The Arduino communicates with the encoder over SPI and communicates with the custom controller with magnitude (analog voltage) and direction (High/Low).

For my brushed motor controllers I found that with higher switching frequencies I could achieve lower motor speeds (not sure why at the moment but I found this empirically) so obviously I maximized my switching speeds. My controller is Atmega328p based and so by default analog writing only occurs at 490 or 980 Hz depending on the pin. However with some timer diddling, I was able to crank that way up to 31Khz and 62 Khz. Plus now I don’t need to hear annoying whining noises. The downside is anything that uses any of the timers is now off by a scaling factor (including things like millis()). For those wondering, these are the bit diddling lines required to get the analog writes at the highest speeds possible on an Atmega328p. Make sure you choose your PWM pins wisely in the beginning. Each timer is associated with 2 analog write pins. I only need 4 pins but happened to have them span all 3 timers (ugh). Timer0 (the first line) is used by millis()/micros() so try to avoid that one if you can help it.

  TCCR0B = TCCR0B & 0b11111000 | 0x01;
  TCCR1B = TCCR1B & 0b11111000 | 0x01;
  TCCR2B = TCCR2B & 0b11111000 | 0x01;

The individual modules handle finding the shortest path to the desired vector. For those not familiar, for a given absolute angle and speed (what I would call a vector), the same vector can be achieved by rotating the module 180 degrees and spinning the wheel in the opposite direction. If the module needs to rotate greater than 90 degrees, it flips the target vector. Additionally, the speed of the wheel is modulated with the cosine of the angle error so that motion in the non-desired directions is minimized.

In terms of implementation, for the shortest target direction, I keep a speed multiplier variable which I multiply by -1 when I choose to use the flipped vector. Cosines on an Arduino are relatively slow so I use a lookup table which is certainly good enough for my speed resolution. In my case a 180 value long array I generated with a 3 line python script is more than good enough (my wheel speeds are not that sensitive).

I use a Nucleo powered by an STM32F446 for the main controller. This communicates with the 4 modules with 4 independent UART lines, and it also reads the IMU (I2C) and SBUS radio (a form of inverted serial). I originally tried having the Nucleo communicate with all 4 modules with one Serial TX line, but the Arduinos benefit from parsing less messages due to their much lower speed. Plus the Nucleo has 6 UART lines so there’s no shortage. I find this graphic extremely useful for identifying Nucleo pin purposes and have had it bookmarked for the last three years. https://os.mbed.com/platforms/ST-Nucleo-F446RE/

If you’re counting communication protocols, there are 5 different ones used in this robot lol. The SBUS, I2C for the IMU, and SPI for the encoder are all well defined communication protocols, but I made up my own form of serial messages to encode the desired vector from the Nucleo, sent to the Arduino.

Messages are of the form <XYYYZZ> where X was an integer indicating module number (or 0 for all modules), YYY represents the angle in degrees, and ZZ represents wheel speed (0.00 -> 0.99). Zeroing commands are sent as <000000>, enable commands are sent as <000100> and disable commands are sent as <000200>. The brackets ensure I don’t have a frame shift in my messages and the messages were designed to be as short as possible due to the limited speed of the Arduino.

I specifically chose the IMU that I did because it performs all necessary filtering in hardware and can produce a clean and reliable set of Euler angles or Quaternions over I2C. In this case I just use one Euler angle for yaw since swerve is basically 2D.

I use the IMU for headless mode or field-centric control. When I engage this mode using a switch on my radio, the robot takes note of its current angle, and now even when it turns, pushing forward on the stick causes it to move forward from its starting angle. This allows for maneuvers like spinning while translating and is done with a single rotation matrix to change x and y velocities from the robot frame to the world frame.

float newYVel = xVel*sin(curYawIMU)+yVel*cos(curYawIMU);
float newXVel = xVel*cos(curYawIMU)-yVel*sin(curYawIMU);

The sbus protocol is a strange form of serial created by some rc airplane companies. It’s 100000 bps inverted serial. I used a logic inverter that Jared recommended, and deadbugged it on top of the nucleo. Then using Austin’s magic DMA register diddling from his SD Card logger I was able to get the sbus data. The DMA is excellent because I can constantly have a chunk of memory with the current sbus values and don’t need to waste processor cycles parsing the sbus serial.

From the sbus receiver I can have up to 16 channels but I currently only use 6. Three channels for X, Y, and Yaw velocities, and 3 switches for Zeroing, Enabling, and Headless mode.

Anyway enough tiny details. If you’re reading this and are working on a swerve and have questions or would like to see some more of the code just fill out the contact form and I’ll do my best to get back to you.

In terms of the future of the swerve, I still haven’t decided what to do with this swerve but I’m extremely happy with how it turned out. Future plans include: attaching a mechanism (like ball yeeter?) to the top, laying out a board to replace my hodge podge of electronics, and swerve cart (I’ve already started the planning for this…)