Added PID control, currently have a compile error