Better rotate to angle & fixed imu reset on cmd

Fixed IMU resetting when it was done with RotatetoAngle. Better
rotatetoangle without huge while loop. Cleaned up.