Implemented PID control, untested