Added switch checking, main loop waits for switch, terminates when switch is off, goal selection