Auton doesn't finish

So my team’s auton works completely sometimes, but sometimes it only does about half of what I have programmed it to do even when I haven’t changed anything in the code. I’m using PID for turning and driving forward and back and I’m resetting the motor encoders and inertial sensor everytime after the robot moves. If anyone know how to fix this it would be very helpful.

Usually in my experience this means your code is stuck in your PID function and not exiting. Because yours is working sometimes I would say it is most likely that your error is not getting small enough to break out of the loop. Check your tuning.

Sometimes something like an if statement that sets the motor speed to a min value if it goes below said value can help.

For example:

if(motorSpeed < 20) {
motorSpeed = 20;