Further refactoring, enclosed behaviors into classes, started fixing robot to accomidate.