So we have been working on some swerve drive code and we have come really close to achieving it. Right now however, we have one problem we are not sure how to tackle. We did all of our calculations using this paper we found and after all the calculations the angle of each wheel is in the range of -180 to 180 degrees. The basic overview of how the code works is that we calculate velocities and angles after running calculations using joystick inputs and then command each individual motor its respective wheel velocity/angle. The problem comes when we try to change the angle from say, 170 degrees to -170 degrees, which in reality is a 20 degree angle difference, the motor does almost a whole turn of 340 in the opposite direction to get to that angle.

I was wondering if anyone else that has done swerve has a solution for this problem. Thanks in advance!