nach welchem Algorithmus soll denn dein Cubesolver den Würfel lösen?
Bist du sicher, dass ein dein AVR Mega dann auch dafür ausreicht (cpu speed, RAM, Flash, SD) ?
Soll er den Würfel während der Laufzeit komplett neu berechnen oder soll er auf vorberechnete, bereits optimierte Zugtabellen zurückgreifen?

(ich weiß allerdings auch nicht, was man alles mit G-Code anfangen kann - oder auch nicht.
Ich nutze für den Mega oder den Due nur die Arduino IDE, die auf C/C++ basiert.)