Made optimizations to PID controller