PID controller added to frobit_demo