Wrote updateHeading for Drive module.

updateHeading function implemented, Controller implemented digitally and Ucmd is utilized in a 1:3 ratio when governing PWM duty cycles for differential motor drive