Motor driver software work

Implemented encoder reading/writing over I2C (only tested reading)