Fixed minor functional errors in the i2c module.
Many changes, including basic PD controller.