Help with pathplanner not rotation and going inverted directions

@mjansen4857
Hey we are running into some issues with pathplanner with swerve. We are trying to run two paths. I’ll discuss the first issue with the first path. We are trying to run the following path.


But instead it runs inverted and doesnt turn the full 180 degrees, and it completes a path like this.

Code will be posted below.
Additionally our robot also drifts 20 degrees when completing a straight line path. But we believe this could be due to our PID or our angle motors being on coast.

Thanks for all the help.

What motors are you using?