Started on the RobotIO.h/.cpp files. Still need to figure out a design for the messages