Adds general robot function to a robot library

This library will be used to control the robot and keep all its
functions in one place. It will contains methods for minipulating all of
the hardware attached to the robot, but no actual contol logic.
Updates the main sketch to use the new robot library