Integration Pain
I got all the subsystems working separately but the extremely fun part of integrating them all together was the first time I truly understood why engineers hate integration. There were a plethora of pin conflicts underlying the hardware since many of the pins on the nucleo serve multiple purposes.
One example of this was the adafruit motor shield’s i2c bus interfering with the front left drive motor’s FOC math since PB8 and PB9 are connected to the sda and scl pins on the Arduino headers of the nucleo. As you can see from the extremely janky wiring in the photo, just connecting the shield so it gets power on the other side of headers and manually connecting the shield to a separate i2c bus using jumper wires, I was able to finally make all the subsystems work together.
This was just one example of the many problems that arose, hence why there are almost 12 hours being logged in this post, but this project is finally on the last stretch. I did also make a lot of other progress like confirming the swerve kinematics work along with the flysky controller for inputs :)

Things Left to Do on Hardware

• Cut off the sda and scl pins on the motor shield and solder a jumper wire to the new i2c bus

Things Left to Do on Software

• Tune the pid for FR, BR, and BL turn motors
• Integrate offsets into the steering angle for the motors
• Integrate the MPU6050 readings for field oriented swerve control