Added goals and states. Switch between robots
ui improvements, some error checking