Added PID control-- currently poorly tuned