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