Swerve Module Programming Optimization

Hello all,

This year my team decided to go with a swerve drive for our robot, something our team had worked on during the offseason in the past. While our robot drives well overall, there are a few things we would like to optimize that I cannot figure out how to fix. We are using the Andymark Swerve and Steer modules with the standard encoders on the steer motors.

  1. For some reason, at the beginning of the match, the wheels will always turn from the 0 position on the encoders to 180 degrees, which we cannot figure out. This only happens when we first drive forward at the beginning of a match, during which time the encoders are reset to 0. What could be causing this?

  2. When quickly changing direction, going from forward to strafing, for example, the robot is a little jerky in that moment. Is there a good way to fix this?

  3. Finally, the robot does not go perfectly straight even when all of the encoders are at the 0 position. We reset their position before any use so there is a little bit of error possible, but the robot veers a lot. Does anyone know any good ways to fix this? We are using the PigeonImu if that helps.

Our code can be found here: https://github.com/CAPS-Robotics/2017-Steamworks/tree/576a252f1765546ef2b447177276225fa49c29f1/src/Subsystems
The main classes to look at are SwerveModule and Drivetrain.