TMC222 Ansteuerung

Hallo,
ich benutze einen TMC222 um die Schrittmotoren von meinem Moving Head 
per DMX zu steueren und nutze dazu die TMC222.h . Das funktioniert 
soweit auch. Doch wenn ich die Position in der Endlosschleife ausgebe 
funktioniert das für ein paar Sekunden und dann hängt sich der Atmega 
auf. Im folgenden habe ich versucht immer nur die Position zu setzen 
wenn diese sich verändert hat.

soll = (dmx_array[1] * 66);
if (soll != ist){
        ist = soll;
  SetPosition(soll);
}

(dmx_array[1] * 66 weil ich 16830 Schritte für 360° brauche)

So funktioniert es auch, aber 255 Auflösung sind zu niedrig deshalb 
nutze ich einen zweiten Kanal um feinere Bewegungen zu realisieren.

soll = (dmx_array[1] * 66 + dmx_array[3] * 0.2);
if (soll != ist){
  ist = soll;
  SetPosition(soll);
}

Wenn ich dies so auf den Atmega flashe habe ich das gleich Ergebnis wie 
wenn ich den SetPosition Befehl einfach so in die Endlosschleife tue und 
der Motor ruckelt auch noch ein paar Schritte hin und her.

Kann mir bitte einer sagen was an dem Code falsch ist ?

Danke im vorraus :)