Finalized TINAH Motor Control code

This includes both the servomotor and the regular motor/H-bridge code.