Fix tests on real robot, fix Travis building

- All tests now pass both in simulation and on real robot
- Travis building is fixed