Add in code to actually init the modules.

[schreiaj]

Also sets the CAN IDs for the drive. Partially finishing #8

This also fixes some runtime errors (a collection of them) so is required before running on robot