Create Team C Full DriveCode

Our Official code for Team C robot. No Autonomous yet because Matteo is writing this separately.