Removed zero-return case, some parameter tuning for arm controller