added main skeleton
added displacement calculation
added i2c comms setup
added motor config code