Made the code a bit more modular, added rgb functions, pwm, and i2c initialization.  Tested on the quad to ensure that it worked.