combined P controller and ORCA in orca.cpp