With my lack of C++ knowledge (I think its C++), I can't critique your code. I can tell you, based on my experience with PID controllers, that what you are looking to do is probably better accomplished using only a PID controller, with the sum of all of the encoder values as the process variable and the desired angle as the setpoint. A PID controller will automatically accelerate the motor to maximum (if needed), coast at maximum for however long it takes to get close to the point ('close' is changed by tuning the PID), and then decelerate until it hits the point you want, which seems to be what you are looking to do. If tuned properly, the PID loop should not only be very accurate and fast at getting the motor to the specified point, but it should also hold the motor at the point if something tries to move it.
If you need help writing or tuning a PID controller (and I would recommend writing it yourself), there are plenty of websites that can help with that. Here are a few: