Hi,

das sieht ja gut aus: Hardware fertig ...

Software:
Wird auch klappen. Für den Anfang brauchst du als Basis eine Kombination aus RP6Control_09_Move.c und RP6Control_08_I2CMaster.c.
Damit würde ich erst mal starten ...