Fixed angle error on PID input and various other bug fixes