Start robot program; fix some bugs