Mini Plotter

Hola
Por favor, alguien podria echarme una mano con este problema?
Ahora tengo mas datos...
He estado haciendo pruebas porque esto me sonaba a tema de posicionamiento de inicio y asi parece ser... si pongo el inicio tanto del eje x como del y en un extremo el error es de solo 2-3 mm en la primera pasada y luego el resto de pasadas las hace correctamente, y si lo posiciono a medias o en el otro extremo se agrava muchisimo mas.
El tema es que el mas correcto de todos esta justo en el tope de los dos ejes...
Creo que el tema esta en esta parte del codigo pero no estoy seguro de sea asi...
Mañana de todas formas probare a hacer alguna modificacion

// Drawing robot limits, in mm
// OK to start with. Could go up to 50 mm if calibrated well. 
float Xmin = 0;
float Xmax = 40;
float Ymin = 0;
float Ymax = 40;
float Zmin = 0;
float Zmax = 1;

float Xpos = Xmin;
float Ypos = Ymin;
float Zpos = Zmax;

Si pusiese Xmin =2, solucionaria algo?
o tendria que modificar los max?