80% functional. New pen head and more accurate code including doubles
and mm size steps.