Простая функция для перемещения по двум осям

Вот набросал на python небольшую функцию - основную для работы. Эта функция обеспечивает перемещение из точки x0 y0 в точку x1 y1. При этом проверяется состояние концевых выключателей. Вы можете легко изменить ее под свои нужды, сложного в языке программы ничего нет:

import parallel
import time

Xcu=0
Ycu=0
Xnew=0
Ynew=0
DirXp=1
DirYp=16
Xpulse=2
Ypulse=8
pausetime=0.001
stepkoefX=0.01
stepkoefY=0.01
p=parallel.Parallel()

def GoToXYfunc(x0,y0,x1,y1):
----# absolute moving
----ControlWord=0
----DirX=DirXp
----DirY=DirYp
----dX=0
----dY=0
----dX=x1-x0
----dY=y1-y0
----XSteps=abs(int(dX/stepkoefX))
----YSteps=abs(int(dY/stepkoefY))
----if dX<0:DirX=DirX.__xor__(DirXp)
----if dY<0:DirY=DirY.__xor__(DirYp)
----if XSteps>=YSteps:
--------MaxSteps=XSteps
----else:
--------MaxSteps=YSteps
----ControlWord=ControlWord.__or__(DirX)
----ControlWord=ControlWord.__or__(DirY)
----for i in xrange(MaxSteps):
--------Xph=p.getInAcknowledge()
--------Yph=p.getInBusy()
--------if i<=XSteps and Xph!=0:
------------# step by X
------------ControlWord=ControlWord.__or__(Xpulse)
--------if i<=YSteps and Yph!=0:
------------# step by Y
------------ControlWord=ControlWord.__or__(Ypulse)
------------# zeroing Step Levels
--------p.setData(ControlWord)
--------time.sleep(pausetime)
--------if i<=XSteps and Xph!=0:
------------ControlWord=ControlWord.__xor__(Xpulse)
--------if i<=YSteps and Yph!=0:
------------ControlWord=ControlWord.__xor__(Ypulse)
--------p.setData(ControlWord)
----return (x1,y1)

Xnew=-10
Ynew=10
Xcu=0
Ycu=0
print GoToXYfunc(Xcu,Ycu,Xnew,Ynew)