Hey guys we could use some help regarding swerve trajectory following. We used the WPILIB command example code and converted it to TimedRobot code. The github is below. The robots initial pose is (in meters) (-3, -2, 0) and goes to the right trench to pick up 3 balls. Our problem is that the robot starts off going the correct x and y directions, and the accelerates in the opposite direction (positive x). when we teleop drive or push the robot, the odometry seems to align with what we do. Any help would be appreciated. Any questions are welcome.
Swerve Drive is operated utilizing SD MK2 swerve modules (@PatrickW) and driven with 4 falcon drive motors and 4 minicim steering motors.