Changes on robot. Still needs tests in simulation.