def __init__(self, xy: XYPoint, ship_angle: float, width: int, height: int, n_sensors: int, sensor_resolution: int, sensor_length: int, sensor_width: float): self.xy = xy self.angle = ship_angle self.width = width self.height = height self.n_sensors = n_sensors self.sensor_resolution = sensor_resolution self.sensor_width = sensor_width self.step = 5 self.turn_angle = pi / 12 self.max_eye_length = distance(XYPoint(0, 0), XYPoint(width, height)) self.eye_length = min(sensor_length, self.max_eye_length) eye_max_angle = self.sensor_width if n_sensors > 1: eye_step = (eye_max_angle) / (n_sensors - 1) else: eye_step = 0 self.eyes = [] # add the leftmost eye first, working our way from +sensor_width/2 to -sensor_width/2 for i in range(0, n_sensors): if n_sensors == 1: angle = 0 else: angle = eye_max_angle / 2 - eye_step * i self.eyes.append( Eye(xy, ship_angle, angle, eye_step, self.eye_length, self.width, self.height))
def move_to_point(goal_pos, current_position, current_angle, magnitude): distance = mathutils.distance(current_position, goal_pos) if (distance < 60): return True diff = (goal_pos[0] - current_position[0], goal_pos[1] - current_position[1]) goal_angle = math.atan2(diff[1], diff[0]) angle_diff = mathutils.wrap_angle(goal_angle - current_angle) print(angle_diff) angle = angle_diff / (math.pi) + 1 print(angle) left = magnitude * (angle - 0.5) right = magnitude * (1.5 - angle) print(left, right) commands.set_speed(int(left), int(right)) return False
def update(self, all_apples: []) -> np.ndarray: ship = self.ship self.screen.fill(0.0) ship_angle = self.ship.angle for a in all_apples: # the angle from the ship-xy to the object-xy object_angle = (a.xy - self.ship.xy).angle() # this is the angle from the left edge of the sensor field (ship_angle + sensor_width / 2) # to the object. This is stated as a positive angle in the clockwise direction. net_angle = reduce_angle((ship_angle + self.sensor_width / 2) - object_angle) d = distance(a.xy, self.ship.xy) # use net_angle instead of sensor_index > 0 because int(-.5) = 0 if (net_angle >= 0 and net_angle < self.sensor_width and d < ship.eye_length): # take the fraction of the distance through the sensor field and translate that into an integer # corresponding to the nearest sensor sensor_idx = int(net_angle / self.sensor_width * self.n_sensors) col = min( self.sensor_resolution - 1, int(d / ship.eye_length * self.sensor_resolution + .5)) if a.ripe(): self.screen[sensor_idx * 3 + 0, col] += 1 else: self.screen[sensor_idx * 3 + 1, col] += 1 for idx, eye in enumerate(ship.eyes): n_blocked = int((eye.length - eye.actual_length) / eye.length * self.resolution + .5) for d in range(n_blocked): self.screen[idx * 3 + 2, self.resolution - d - 1] = 1 return self.screen
def can_eat(self, o) -> bool: if distance(o.xy, self.xy) < o.radius + 3: return True else: return False
import math import sys commands.open_connection(("0.0.0.0", 55555)) commands.set_pid_params(10, 4, 2) speed = int(sys.argv[1]) delays = [2.2, 1.6, 1.0 , 2.8] avg_speed = 0 for delay in delays: p_init = commands.where_robot()["center"] if delay == 1.0: speed = -speed print(speed) commands.set_speed(speed, speed) sleep(1.5) commands.set_speed(0, 0) sleep(1) p_next = commands.where_robot()["center"] diff_magnitude = math.fabs(mathutils.distance(p_init, p_next)) print(diff_magnitude) avg_speed += diff_magnitude / delay print(str(avg_speed / len(delays)) + " px/s") commands.close_connection
def update(self): self.p1 = self.xy self.p2 = self.project(self.angle, self.length) self.actual_length = distance(self.p1, self.p2)