Własny program do sterowania przy wykorzystaniu gcode

Witam, mam nadzieje, że znajdzie się tutaj ktoś kto pomoże mi ogarnąć pewien problem. Od pewnego czasu buduje ploter. Wszystko od podstaw wraz z analizą gcode i o tą analizę najbardziej mi chodzi. Napisałem program w którym wczytujemy gcode, wyświetla nam się podgląd danego pliku. Następnie analizowana jest linia po linii i wyciągane są z niej informacje o wartości przesunięć w osiach X i Y. Działa praktycznie wszystko, wysyłanie ramki na sterownik, potem tą ramkę analizuje i zadaje odpowiednią ilość kroków na każdą oś. Jeśli ilość kroków podam bezpośrednio na sterowniku, bez wysyłania tego po usb na arduino, to wszystko śmiga i układ realizuje zadana trajektorie. A więc sterownik działa spoko. Raczej nie wchodzi w grę to, że dane podczas wysyłania na sterownik ulegają jakieś zmianie, bo sprawdzałem i wyglądały tak samo.
Ale wracając do tej analizy gcode. Sprawa wygląda tak, na 1mm przypada 10 kroków, zatem jeśli mamy taki kod(4 linijki tylko);
G1 X0.0 Y0.0 F3500.00
G1 X31.02 Y76.35 F3500.00
G1 X35.77 Y76.98 F3500.00
G1 X42.74 Y78.10 F3500.00
To dla X w pierwszym kroku będzie 310,2 kroków. Ale 0,2 kroku nie może wykonać dlatego trzeba to gdzieś zapamiętać i wziąć pod uwagę, że X raz może być na minusie a raz na plusie.
A wiec to 0.2 może się albo zwiększać albo zmniejszać.
W drugim kroku będzie musiał wykonać po odjęciu wartości obecnej od poprzedniej 35.77-31.02 = 4.75 mm a zatem 47.5 kroków. A więc 0,5 leci do bufora i mamy w nim już 0.2+0,5 = 0.7.
W kroku trzecim po odjęciu wychodzi 69,7 kroków. A wiec w buforze mamy 1,4. Zatem jeden pełny krok i 0,4. Z bufora odejmujemy 1. I dodajemy jedynkę a wiec nie 69 kroków tym razem, tylko 70.
Tak właśnie robiłem i to kurde nie działa:/.
Próbowałem to na różne sposoby uwzględniać ale nie chce wyjść. Nie mogę rozgryźć co robię źle :/
Tutaj kod do moich pseudo wyliczeń, mam nadzieje że będzie w miarę zrozumiały Tym minusem który przypisuje w y się nie przejmujcie obraz jest po prostu rysowany jako odbicie lustrzane, poprawię to później
http://pastebin.com/L9i5t5za
A poniżej film jak to działa, w pierwszej części trajektoria zadana na samym uC, w drugiej kod wysyłany po analizowaniu gcode.




A więc czy ktoś z szanownego grona mógłby mi powiedzieć w którym miejscu źle wyliczam te kroki? Bo napewno wyliczane nie są dobrze skoro rysunek mi się rozjeżdża. Gcode jest generowany z Inkscape.

Jeżeli ploter ma rozdzielczosc 0.1 mm to co robi to 0.01 mm w kodzie ?

Gcode jest generowany z inkscape. A silnik akurat ma tak a rozdzielczość i tego nie przeskoczę. Pytanie tyczy się tego co zrobic aby dzialalo. Bo jeśli nie ma 0.01 wartosci to smiga. Testowalem.