Fix controller. Tested with one arm