def excute(D):
#step 1:
destPoint = array([D[0],D[1]])
currentPosition = array([[0],[0],[0]])
# go obs and get current position
#destPoint = dest();
#currentPosition = obs();
currentPosition = mover.getLocation()
while not mover.getSet():
time.sleep(0.05)
currentPosition = mover.getLocation()
currentPosition = (0,0,0)
dx = destPoint[0] - currentPosition[0]
dy = destpoint[1] - currentPosition[1]
angle = math.atan2(dy,dx) - currentPosition[2]
if angle > math.pi: #probabbly buggy have to test this code might have angle
angle = math.pi - angle
turning(angle)
dist = sqrt(pow((destpoint[1]-currentPosition[1]),2) + pow(destpoint[0]-currentPosition[0]),2)
dist = dist/3
goal = array([[currentPosition[0] + dist*cos(currentPosition[2])],[currentPosition[0] + dist*sin(currentPosition[2])],[0]])
driving(dist);
#step 2:
currentPosition = mover.getLocation()
dx = destPoint[0] - currentPosition[0]
dy = destpoint[1] - currentPosition[1]
angle = math.atan2(dy,dx) - currentPosition[2]
if angle > math.pi: #probabbly buggy have to test this code might have angle
angle = math.pi - angle
turning(angle)
dist = sqrt(pow((destpoint[1]-currentPosition[1]),2) + pow(destpoint[0]-currentPosition[0]),2)
dist = dist/3
goal = array([[currentPosition[0] + dist*cos(currentPosition[2])],[currentPosition[0] + dist*sin(currentPosition[2])],[0]])
driving(dist);
#step 2:
currentPosition = mover.getLocation()
while not mover.getSet():
time.sleep(0.05)
currentPosition = mover.getLocation()
dx = destPoint[0] - currentPosition[0]
dy = destpoint[1] - currentPosition[1]
angle = math.atan2(dy,dx) - currentPosition[2]
if angle > math.pi: #probabbly buggy have to test this code might have angle
angle = math.pi - angle
turning(angle)
dist = sqrt(pow((destpoint[1]-currentPosition[1]),2) + pow(destpoint[0]-currentPosition[0]),2)
dist = dist/2
goal = array([[currentPosition[0] + dist*cos(currentPosition[2])],[currentPosition[0] + dist*sin(currentPosition[2])],[0]])
driving(dist);c
