Initial implementation of all subsystems and commands for ShifterBot