def set_target_position_lidar(car, sock, x, y, z, max_speed=50):
command = tcns.listen_udp(sock)
posx = 0
posy = 0
posz = 0
lastx = x
lasty = y
lastz = z
x = x - car[0]
y = y - car[1]
z = z - car[2]
print('xx {} yy {} zz {}'.format(x, y, z))
dis = (x**2 + y**2)**0.5 # Encoder
angle = tcnp.vector_angle(x, y)
loop_int = int(dis / max_speed)
loop_float = dis / max_speed
dis_left = int(max_speed * (loop_float - loop_int))
for i in range(loop_int):
if command != None:
car = lidar_evade_velocity(car, command)
car = set_target_position_lidar(car, sock, lastx + car[0],
lasty + car[1], z, max_speed)
print('evading')
return 0
else:
if abs(x) > 0:
car[0] = int(round(max_speed * math.cos(angle), 0))
if abs(y) > 0:
car[1] = int(round(max_speed * math.sin(angle), 0))
if abs(z) > 0:
if z > 0:
car[2] = max_speed
if z < 0:
car[2] = -max_speed
#print("x {} y {} z {}".format(posx,posy,posz))
tcnp.move_to_coordinate(car)
time.sleep(0.006)
car[0] = car[1] = car[2] = 0
tcnp.move_to_coordinate(car)
car[0] = lastx
car[1] = lasty
car[2] = lastz
return car
def set_target_velocity(car, x, y, z, max_speed=50):
posx = 0
posy = 0
posz = 0
lastx = x
lasty = y
lastz = z
x = x - car[0]
y = y - car[1]
z = z - car[2]
print('xx {} yy {} zz {}'.format(x, y, z))
dis = (x**2 + y**2)**0.5 # Encoder
angle = tcnp.vector_angle(x, y)
loop_int = int(dis / max_speed)
loop_float = dis / max_speed
dis_left = int(max_speed * (loop_float - loop_int))
for i in range(loop_int):
if abs(x) > 0:
car[0] = int(round(max_speed * math.cos(angle), 0))
if abs(y) > 0:
car[1] = int(round(max_speed * math.sin(angle), 0))
if abs(z) > 0:
if z > 0:
car[2] = max_speed
if z < 0:
car[2] = -max_speed
#print("x {} y {} z {}".format(posx,posy,posz))
tcnp.move_to_coordinate(car)
time.sleep(0.006)
car[0] = car[1] = car[2] = 0
tcnp.move_to_coordinate(car)
car[0] = lastx
car[1] = lasty
car[2] = lastz
return car