My team has been converting our swerve drive code to be able to use odometry, and we have run into an issue with the rotation motors. The math for calculating angle is correct, and all of the motors will point in the same, correct direction. However, when the motor needs to rotate, it will only spin clockwise. Even if it only needs to rotate 10 degrees to the left, it will rotate 350 to the right. We have narrowed down this issue to the PID for the rotation motor only outputting positive values, but we are unsure why. All of the relevant code should be here: Technodogs-2021/SwerveModule.java at auto_odometry · Brighton-Technodogs/Technodogs-2021 · GitHub
Any help would be much appreciated.