Added PID routine for testing