Wieso machst du am Anfang schon so komplexe Sachen??
Ich glaube nicht dass irgendjemand in diesem Forum dir ein komplettes Programm schreiben wird.
Versuch doch zuerst mal die Megas über RS232 an den PC anzuschliessen und diverse LEDs vom Terminal aus zu steuern und verwende PWM und Timer.......................
Du wirst sicherlich so wie ich Unterstützung bekommen, die Leute hier in diesem Forum sind diesbezüglich SEHR HILFREICH.
Mach immer kleine Sprünge und du wirst zum Robotervisionär.

MFG

Bertl