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)