Significant fixes to robot movement