Initial Competition Ready

This code has been fairly thoroughly tested to work with the robot in its current state