Added PID testing code, tuning in progress