In your Robot class, you are initializing an OI object before you are making your subsystems. Therefore, the subsystems you are passing to your OI class are null. Try initializing your OI object after you initialize your subsystems.
Your DriveTrain object is never initialized before using it here, and therefore is null, which causes this error to be thrown in your TurnToAngle command.
The motor safety issue usually indicates that you have some sort of loop in one of your methods that takes too many milliseconds. The only time consuming thing I can see is your telemetry, but it shouldn’t be that long. Just to be sure, see if commenting out the ouptutTelemetry methods called from the periodic parts of Robot.java solves the issue. If not, to help the search - is this happening in teleop, or some other state?
Commenting that out didn’t change the error message. My turn to angle method works perfectly fine so it must be something within the drivewithjoysticks command. This is in teleop
Do you ever initialize your DriveWithJoySticks command? Looking at your code, I don’t see it initialized anywhere, so it really should be giving you a null pointer exception as well. Try initializing your DriveWithJoySticks command in your robotInit method before you call driveTrain.establishDefaultCommand()
So sorry to bother you again but I was curious if any of you could help explain why I can only run my turn to angle command once? When I run it the first time it works fine, then when I press it again only the gyro angle resets. It seems that the PID controller will not re-enable a second time.