class Robot(WorldObject): # Robot must fit into a 350mm cylinder, which here means that it should not exceed a square of 49x49 pixels more or less. Hence the width/height parameters. def __init__(self, world, name, center, width = 35, height = 45, beacon_point = Point(0, 0)): # For simple bounding circle computations let the radius be half diagonal WorldObject.__init__(self, center, sqrt(width**2 + height**2)/2) self.wr = width/2 self.hr = height/2 self.world = world self.name = name self.beacon_point = beacon_point # "Forward" is the unit direction vector where the robot is facing # (0, 1) is the "default" (height along the Y axis) self.forward = Point(0, 1) self.left = Point(-self.forward.y, self.forward.x) # Those are state parameters self.leftSpeed = 0 self.rightSpeed = 0 # Camera sensor parameters self.CAMERA_DEPTH = 120 self.CAMERA_SIDE = 100 # Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors) self.data_lock = thread.allocate_lock() # Whether there's a ball in the grabber self.grabbed_ball = None self.grabbed_ball_lock = thread.allocate_lock() def draw(self, screen): # Center point draw.line(screen, (0,0,0), (self.center - self.left*5).as_tuple(), (self.center + self.left*5).as_tuple()) draw.line(screen, (0,0,0), self.center.as_tuple(), (self.center + self.forward*8).as_tuple()) # Sensor edges draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple()) draw.line(screen, (255,0,0), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple(), \ (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) # Beacon line if (self.beacon()): draw.line(screen, (255, 100, 0), self.center.as_tuple(), self.beacon_point.as_tuple()) # Wheels for side in [1, -1]: draw.line(screen, (0,0,0), (self.center + self.left*self.wr*side - self.forward*8).as_tuple(), (self.center + self.left*self.wr*side + self.forward*8).as_tuple(), 5) # Sides for (f,t) in self.edges(): draw.line(screen, (0,0,0), f.as_tuple(), t.as_tuple()) def edges(self): """Enumerate edges as tuples ((x,y), (x,y)) in clockwise order [assuming mathematical coordinates]""" leftback = self.center + self.left*self.wr - self.forward*self.hr leftfront = self.center + self.left*self.wr + self.forward*self.hr rightfront = self.center - self.left*self.wr + self.forward*self.hr rightback = self.center - self.left*self.wr - self.forward*self.hr yield (leftback, leftfront) yield (leftfront, rightfront) yield (rightfront, rightback) yield (rightback, leftback) def rotate(self, angle): self.forward.rotate(angle) self.left.rotate(angle) def simulate(self): # This is a hack which only works at small simulation steps leftTurn = (self.leftSpeed - self.rightSpeed)/self.wr/2 forwardMove = (self.leftSpeed + self.rightSpeed)/2 self.v = self.forward*forwardMove if (self.v != 0): self.center.add(self.v) if (leftTurn != 0): self.forward = self.forward + self.left*leftTurn self.forward.normalize() self.left = Point(-self.forward.y, self.forward.x) # If there is a grabbed ball, carry it around with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.v = Point(0,0) self.grabbed_ball.center = self.center + self.forward*self.grabbed_forward + self.left*self.grabbed_left # Precompute "edge walls", those will be useful in collision checks self.edge_walls = [Wall(e[1], e[0]) for e in self.edges()] all_x = [w.p1.x for w in self.edge_walls] all_y = [w.p1.y for w in self.edge_walls] def wall_check(self, w): # Robot's wall handling is fairly trivial. If we see we're hitting the wall, we'll nudge back from it m = min([w.dist_to_point(edge[0]) for edge in self.edges()]) if (m < 0): # We need to nudge perpendicular to the wall by distance -m self.center.add(w.normal*(-m)) def collision_check(self, obj): # If it is not a ball, ignore it if not isinstance(obj, Ball): # Just check the "bounding circle" dir = obj.center - self.center dist = dir.norm() if (dist < obj.radius + self.radius): # Nudge either us or them, choose randomly to avoid some ugliness dir_normalized = dir * (1/dist) if (random.randint(0,1) == 0): # Them obj.center.add(dir_normalized*(obj.radius + self.radius - dist)) else: # Us self.center.add(dir_normalized*(dist - (obj.radius + self.radius))) else: # Find which wall is the ball touching for w in self.edge_walls: d = w.dist_to_point(obj.center) - obj.radius if (d > -0.3 and d < 0): # Is the ball within the range of the wall at all? wall_coord = (obj.center - w.p1).inner_product(w.v_normalized) if (wall_coord >= -obj.radius and wall_coord <= w.len+obj.radius): # Yes, it does, something must be done. # First, nudge obj.center.add(w.normal * (-d)) # Second, simulate a rebounce (this is a hack, but it's way easier than considering rotations and stuff) obj.v.add(w.normal * (-d*2)) # ----------- The following are the main commands for the robot ----------------- def wheels(self, left, right): """ Sets the wheel speed. Left and right must be numbers -100..100. The speed is given in 'pixels per second'. 100 pixels per second is 0.5 m/s. """ with self.data_lock: self.leftSpeed = left/1000.0 self.rightSpeed = right/1000.0 def grab(self): "If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'" if self.grabbed_ball is not None: return for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.hr + b.radius + 3: # See whether the ball is within the front edge v_left = self.left.inner_product(v) if (abs(v_left) < self.wr - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = b b.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return def beacon(self): "Returns true if cos(angle) to beacon is > 0.99" dbeacon = self.beacon_point - self.center dbeacon.normalize() return dbeacon.inner_product(self.forward) > 0.99 def shoot(self): "If there is a grabbed ball, the ball is shot off" with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.center = self.center + self.forward*(self.grabbed_forward + 10) + self.left*self.grabbed_left # Shoots the ball at 400 pixels per second (2.0 m/s) self.grabbed_ball.v = self.forward * 0.4 self.grabbed_ball = None def camera(self): "This is the 'camera' sensor. If any ball is found in the 'camera triangle', the distance and bearing to it are reported (with a random 10% noise)" "If several balls are found, one of them is reported (typically it is a stable solution)" for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.CAMERA_DEPTH: # See whether the ball is within the triangle v_left = self.left.inner_product(v) tan = abs(v_left)/v_forward if (tan < float(self.CAMERA_SIDE)/self.CAMERA_DEPTH): # The ball is inside return (v_forward*random.uniform(0.9,1.1), v_left*random.uniform(0.9,1.1)) return None
class Robot(WorldObject): # Robot must fit into a 350mm cylinder, which here means that it should not exceed a square of 49x49 pixels more or less. Hence the width/height parameters. def __init__(self, world, name, role): RADIUS = 130/5 # The diameter of the telliskivi robot is 260mm. In pixels it makes a radius of 26 self.WHEEL_RADIUS = 105/5 self.FORWARD_EDGE_FRONT = 85/5 self.FORWARD_EDGE_LEFT = 100/5 # 98.86mm CENTER = Point(12+RADIUS, 12+RADIUS) if (role == "TOPLEFT") else Point(world.width - 12 - RADIUS, world.height - 12 - RADIUS) WorldObject.__init__(self, CENTER, RADIUS) self.world = world self.name = name self.wr = RADIUS self.hr = RADIUS self.beacon_point = Point(world.width + 50, world.cy) if role == "TOPLEFT" else Point(-50, world.cy) self.beacon_point_en = Point(0, world.cy) if role == "TOPLEFT" else Point(world.width, world.cy) self.goal_center = Point(world.width, world.cy) if role == "TOPLEFT" else Point(0, world.cy) # "Forward" is the unit direction vector where the robot is facing # (0, 1) is the "default" (height along the Y axis) self.forward = Point(0, 1) self.left = Point(-self.forward.y, self.forward.x) if role == "TOPLEFT": self.rotate(3.1415/2) else: self.rotate(-3.1415/2) # Those are state parameters self.leftSpeed = 0 self.rightSpeed = 0 # Camera sensor parameters self.CAMERA_DEPTH = 1000 # Let's say the camera sees as far as the end of the field (~5 meters = 1000 px) self.CAMERA_SIDE = 340 # The camera's side angle is 20 degree, i.e. at 1000 pixels it stretches to 340px # Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors) self.data_lock = thread.allocate_lock() # Whether there's a ball in the grabber self.grabbed_ball = None self.grabbed_ball_lock = thread.allocate_lock() def draw(self, screen): # Center point draw.line(screen, (0,0,0), (self.center - self.left*5).as_tuple(), (self.center + self.left*5).as_tuple()) draw.line(screen, (0,0,0), self.center.as_tuple(), (self.center + self.forward*8).as_tuple()) # Sensor edges draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple()) draw.line(screen, (255,0,0), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple(), \ (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) # Circle # Angle we're pointing to: fwdangle = atan2(-self.forward.y, self.forward.x) fromangle = fwdangle + 0.86 toangle = fwdangle - 0.86 if (toangle < 0): toangle += 2*3.1415 if (fromangle > toangle): fromangle -= 2*3.1415 draw.arc(screen, (0,0,0), Rect(self.center.x - self.radius, self.center.y - self.radius, 2*self.radius, 2*self.radius), fromangle, toangle, 1) # Wheels for side in [1, -1]: draw.line(screen, (0,0,0), (self.center + self.left*self.WHEEL_RADIUS*side - self.forward*(30/5)).as_tuple(), (self.center + self.left*self.WHEEL_RADIUS*side + self.forward*(30/5)).as_tuple(), 5) # Sides for (f,t) in self.edges(): draw.line(screen, (0,0,0), f.as_tuple(), t.as_tuple()) def edges(self): """Enumerate edges as tuples ((x,y), (x,y)) in clockwise order [assuming mathematical coordinates]""" frontleft = self.center + self.forward*self.FORWARD_EDGE_FRONT + self.left*self.FORWARD_EDGE_LEFT frontright = self.center + self.forward*self.FORWARD_EDGE_FRONT - self.left*self.FORWARD_EDGE_LEFT yield (frontleft, frontright) def rotate(self, angle): self.forward.rotate(angle) self.left.rotate(angle) def simulate(self): # This is a hack which only works at small simulation steps leftTurn = (self.leftSpeed - self.rightSpeed)/self.WHEEL_RADIUS/2 forwardMove = (self.leftSpeed + self.rightSpeed)/2 self.v = self.forward*forwardMove if (self.v != 0): self.center.add(self.v) if (leftTurn != 0): self.forward = self.forward + self.left*leftTurn self.forward.normalize() self.left = Point(-self.forward.y, self.forward.x) # If there is a grabbed ball, carry it around with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.v = Point(0,0) self.grabbed_ball.center = self.center + self.forward*self.grabbed_forward + self.left*self.grabbed_left # Precompute "edge walls", those will be useful in collision checks self.edge_walls = [Wall(e[1], e[0]) for e in self.edges()] all_x = [w.p1.x for w in self.edge_walls] all_y = [w.p1.y for w in self.edge_walls] def wall_check(self, w): # Robot's wall handling is fairly trivial. If we see we're hitting the wall, we'll nudge back from it m = w.dist_to_point(self.center) - self.radius if (m < 0): # We need to nudge perpendicular to the wall by distance -m self.center.add(w.normal*(-m)) def collision_check(self, obj): # If it is not a ball, ignore it if not isinstance(obj, Ball): # Just check the "bounding circle" dir = obj.center - self.center dist = dir.norm() if (dist < obj.radius + self.radius): # Nudge either us or them, choose randomly to avoid some ugliness dir_normalized = dir * (1/dist) if (random.randint(0,1) == 0): # Them obj.center.add(dir_normalized*(obj.radius + self.radius - dist)) else: # Us self.center.add(dir_normalized*(dist - (obj.radius + self.radius))) else: # First see whether the ball is within the radius dir = obj.center - self.center d = dir.norm() - obj.radius - self.radius if (d >= 0): return # Otherwise, check whether the ball is within the front edge part front_coord = dir.inner_product(self.forward) if (front_coord < self.FORWARD_EDGE_FRONT): # No, it is a side collision, rebound the ball dir.normalize() obj.center.add(dir * (-d)) obj.v.add(dir * (-d*2)) return # The ball might be touching the front edge, check it d = front_coord - self.FORWARD_EDGE_FRONT - obj.radius if (d < 0): left_coord = dir.inner_product(self.left) if (abs(left_coord) < self.FORWARD_EDGE_LEFT): # Kick from the front edge obj.center.add(self.forward * (-d)) # Second, simulate a rebounce (this is a hack, but it's way easier than considering rotations and stuff) obj.v.add(self.forward * (-d*2)) # ----------- The following are the main commands for the robot ----------------- def wheels(self, left, right): """ Sets the wheel speed. Left and right must be numbers -100..100. 100 is 1 m/s. """ with self.data_lock: self.leftSpeed = left/500.0 # leftSpeed and rightSpeed are in pixels per millisecond 1m/s = 1000mm/s = 200px/s = 0.2px/ms self.rightSpeed = right/500.0 # Hence, 100 input units are mapped to --> 0.2 leftSpeed/rightSpeed units. def grab(self): "If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'" if self.grabbed_ball is not None: return for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.FORWARD_EDGE_FRONT + b.radius + 3: # See whether the ball is within the front edge v_left = self.left.inner_product(v) if (abs(v_left) < self.FORWARD_EDGE_LEFT - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = b b.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return def beacon(self): "Returns true if cos(angle) to beacon is > 0.99" dbeacon = self.beacon_point - self.center dbeacon_forward = self.forward.inner_product(dbeacon) dbeacon_left = self.left.inner_product(dbeacon) dbeacon_en = self.beacon_point_en - self.center dbeacon_en_forward = self.forward.inner_product(dbeacon_en) dbeacon_en_left = self.left.inner_product(dbeacon_en) dbeacon.normalize() dbeacon_en.normalize() if dbeacon.inner_product(self.forward) < 0.9: dbeacon_forward = 5000 dbeacon_left = 500 if dbeacon_en.inner_product(self.forward) < 0.9: dbeacon_en_forward = 5000 dbeacon_en_left = 500 return [dbeacon_forward, dbeacon_left, dbeacon_en_forward, dbeacon_en_left] def goal(self): "Returns the location of the goal, in robot coordinates, if it is visible. Otherwise returns 0 0" dgoal = self.goal_center - self.center goal_dist = dgoal.inner_product(self.forward) goal_left = dgoal.inner_product(self.left) if (goal_dist < 1): return (0, 0) elif (abs(goal_left/goal_dist) > float(self.CAMERA_SIDE)/self.CAMERA_DEPTH): # Is the goal within viewing limits? return (0, 0) else: return (goal_dist, goal_left) def shoot(self): "If there is a grabbed ball, the ball is shot off" with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.center = self.center + self.forward*(self.grabbed_forward + 10) + self.left*self.grabbed_left # Shoots the ball at 0.4 pixels per millisecond (2.0 m/s) self.grabbed_ball.v = self.forward * 0.4 self.grabbed_ball = None def camera(self): "This is the 'camera' sensor. If any ball is found in the 'camera triangle', the distance and bearing to it are reported (with a random 10% noise)" "If several balls are found, the closest is reported" v_forward_closest = 5000; v_left_closest = 5000; for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.CAMERA_DEPTH: # See whether the ball is within the triangle v_left = self.left.inner_product(v) tan = abs(v_left)/v_forward if (tan < float(self.CAMERA_SIDE)/self.CAMERA_DEPTH): # The ball is inside if (v_forward < v_forward_closest): v_forward_closest = v_forward v_left_closest = v_left if (v_forward_closest != 5000): return (v_forward_closest*random.uniform(0.9,1.1), v_left_closest*random.uniform(0.9,1.1)) return None
class Robot(WorldObject):
# Robot must fit into a 350mm cylinder, which here means that it should not exceed a square of 49x49 pixels more or less. Hence the width/height parameters.
def __init__(self, world, name, role):
RADIUS = 130 / 5 # The diameter of the telliskivi robot is 260mm. In pixels it makes a radius of 26
self.WHEEL_RADIUS = 105 / 5
self.FORWARD_EDGE_FRONT = 85 / 5
self.FORWARD_EDGE_LEFT = 100 / 5 # 98.86mm
CENTER = Point(12 + RADIUS, 12 +
RADIUS) if (role == "TOPLEFT") else Point(
world.width - 12 - RADIUS, world.height - 12 -
RADIUS)
WorldObject.__init__(self, CENTER, RADIUS)
self.world = world
self.name = name
self.wr = RADIUS
self.hr = RADIUS
self.beacon_point = Point(
world.width +
50, world.cy) if role == "TOPLEFT" else Point(-50, world.cy)
self.beacon_point_en = Point(0,
world.cy) if role == "TOPLEFT" else Point(
world.width, world.cy)
self.goal_center = Point(world.width,
world.cy) if role == "TOPLEFT" else Point(
0, world.cy)
# "Forward" is the unit direction vector where the robot is facing
# (0, 1) is the "default" (height along the Y axis)
self.forward = Point(0, 1)
self.left = Point(-self.forward.y, self.forward.x)
if role == "TOPLEFT":
self.rotate(3.1415 / 2)
else:
self.rotate(-3.1415 / 2)
# Those are state parameters
self.leftSpeed = 0
self.rightSpeed = 0
# Camera sensor parameters
self.CAMERA_DEPTH = 1000 # Let's say the camera sees as far as the end of the field (~5 meters = 1000 px)
self.CAMERA_SIDE = 340 # The camera's side angle is 20 degree, i.e. at 1000 pixels it stretches to 340px
# Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors)
self.data_lock = thread.allocate_lock()
# Whether there's a ball in the grabber
self.grabbed_ball = None
self.grabbed_ball_lock = thread.allocate_lock()
def draw(self, screen):
# Center point
draw.line(screen, (0, 0, 0), (self.center - self.left * 5).as_tuple(),
(self.center + self.left * 5).as_tuple())
draw.line(screen, (0, 0, 0), self.center.as_tuple(),
(self.center + self.forward * 8).as_tuple())
# Sensor edges
draw.line(screen, (255, 0, 0), self.center.as_tuple(),
(self.center + self.forward * self.CAMERA_DEPTH +
self.left * self.CAMERA_SIDE).as_tuple())
draw.line(screen, (255, 0, 0), self.center.as_tuple(),
(self.center + self.forward * self.CAMERA_DEPTH + self.left *
(-self.CAMERA_SIDE)).as_tuple())
draw.line(screen, (255,0,0), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple(), \
(self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple())
# Circle
# Angle we're pointing to:
fwdangle = atan2(-self.forward.y, self.forward.x)
fromangle = fwdangle + 0.86
toangle = fwdangle - 0.86
if (toangle < 0): toangle += 2 * 3.1415
if (fromangle > toangle):
fromangle -= 2 * 3.1415
draw.arc(
screen, (0, 0, 0),
Rect(self.center.x - self.radius, self.center.y - self.radius,
2 * self.radius, 2 * self.radius), fromangle, toangle, 1)
# Wheels
for side in [1, -1]:
draw.line(screen, (0, 0, 0),
(self.center + self.left * self.WHEEL_RADIUS * side -
self.forward * (30 / 5)).as_tuple(),
(self.center + self.left * self.WHEEL_RADIUS * side +
self.forward * (30 / 5)).as_tuple(), 5)
# Sides
for (f, t) in self.edges():
draw.line(screen, (0, 0, 0), f.as_tuple(), t.as_tuple())
def edges(self):
"""Enumerate edges as tuples ((x,y), (x,y)) in clockwise order [assuming mathematical coordinates]"""
frontleft = self.center + self.forward * self.FORWARD_EDGE_FRONT + self.left * self.FORWARD_EDGE_LEFT
frontright = self.center + self.forward * self.FORWARD_EDGE_FRONT - self.left * self.FORWARD_EDGE_LEFT
yield (frontleft, frontright)
def rotate(self, angle):
self.forward.rotate(angle)
self.left.rotate(angle)
def simulate(self):
# This is a hack which only works at small simulation steps
leftTurn = (self.leftSpeed - self.rightSpeed) / self.WHEEL_RADIUS / 2
forwardMove = (self.leftSpeed + self.rightSpeed) / 2
self.v = self.forward * forwardMove
if (self.v != 0):
self.center.add(self.v)
if (leftTurn != 0):
self.forward = self.forward + self.left * leftTurn
self.forward.normalize()
self.left = Point(-self.forward.y, self.forward.x)
# If there is a grabbed ball, carry it around
with self.grabbed_ball_lock:
if self.grabbed_ball is not None:
self.grabbed_ball.v = Point(0, 0)
self.grabbed_ball.center = self.center + self.forward * self.grabbed_forward + self.left * self.grabbed_left
# Precompute "edge walls", those will be useful in collision checks
self.edge_walls = [Wall(e[1], e[0]) for e in self.edges()]
all_x = [w.p1.x for w in self.edge_walls]
all_y = [w.p1.y for w in self.edge_walls]
def wall_check(self, w):
# Robot's wall handling is fairly trivial. If we see we're hitting the wall, we'll nudge back from it
m = w.dist_to_point(self.center) - self.radius
if (m < 0):
# We need to nudge perpendicular to the wall by distance -m
self.center.add(w.normal * (-m))
def collision_check(self, obj):
# If it is not a ball, ignore it
if not isinstance(obj, Ball):
# Just check the "bounding circle"
dir = obj.center - self.center
dist = dir.norm()
if (dist < obj.radius + self.radius):
# Nudge either us or them, choose randomly to avoid some ugliness
dir_normalized = dir * (1 / dist)
if (random.randint(0, 1) == 0):
# Them
obj.center.add(dir_normalized *
(obj.radius + self.radius - dist))
else:
# Us
self.center.add(dir_normalized *
(dist - (obj.radius + self.radius)))
else:
# First see whether the ball is within the radius
dir = obj.center - self.center
d = dir.norm() - obj.radius - self.radius
if (d >= 0):
return
# Otherwise, check whether the ball is within the front edge part
front_coord = dir.inner_product(self.forward)
if (front_coord < self.FORWARD_EDGE_FRONT):
# No, it is a side collision, rebound the ball
dir.normalize()
obj.center.add(dir * (-d))
obj.v.add(dir * (-d * 2))
return
# The ball might be touching the front edge, check it
d = front_coord - self.FORWARD_EDGE_FRONT - obj.radius
if (d < 0):
left_coord = dir.inner_product(self.left)
if (abs(left_coord) < self.FORWARD_EDGE_LEFT):
# Kick from the front edge
obj.center.add(self.forward * (-d))
# Second, simulate a rebounce (this is a hack, but it's way easier than considering rotations and stuff)
obj.v.add(self.forward * (-d * 2))
# ----------- The following are the main commands for the robot -----------------
def wheels(self, left, right):
"""
Sets the wheel speed. Left and right must be numbers -100..100.
100 is 1 m/s.
"""
with self.data_lock:
self.leftSpeed = left / 500.0 # leftSpeed and rightSpeed are in pixels per millisecond 1m/s = 1000mm/s = 200px/s = 0.2px/ms
self.rightSpeed = right / 500.0 # Hence, 100 input units are mapped to --> 0.2 leftSpeed/rightSpeed units.
def grab(self):
"If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'"
if self.grabbed_ball is not None:
return
for b in self.world.objects:
if isinstance(b, Ball):
# First check distance to center
v = b.center - self.center
v_forward = self.forward.inner_product(v)
if v_forward > 0 and v_forward < self.FORWARD_EDGE_FRONT + b.radius + 3:
# See whether the ball is within the front edge
v_left = self.left.inner_product(v)
if (abs(v_left) < self.FORWARD_EDGE_LEFT - 2):
# OK, grab
with self.grabbed_ball_lock:
self.grabbed_ball = b
b.v = Point(0, 0)
self.grabbed_forward = v_forward - 5
self.grabbed_left = v_left
return
def beacon(self):
"Returns true if cos(angle) to beacon is > 0.99"
dbeacon = self.beacon_point - self.center
dbeacon_forward = self.forward.inner_product(dbeacon)
dbeacon_left = self.left.inner_product(dbeacon)
dbeacon_en = self.beacon_point_en - self.center
dbeacon_en_forward = self.forward.inner_product(dbeacon_en)
dbeacon_en_left = self.left.inner_product(dbeacon_en)
dbeacon.normalize()
dbeacon_en.normalize()
if dbeacon.inner_product(self.forward) < 0.9:
dbeacon_forward = 5000
dbeacon_left = 500
if dbeacon_en.inner_product(self.forward) < 0.9:
dbeacon_en_forward = 5000
dbeacon_en_left = 500
return [
dbeacon_forward, dbeacon_left, dbeacon_en_forward, dbeacon_en_left
]
def goal(self):
"Returns the location of the goal, in robot coordinates, if it is visible. Otherwise returns 0 0"
dgoal = self.goal_center - self.center
goal_dist = dgoal.inner_product(self.forward)
goal_left = dgoal.inner_product(self.left)
if (goal_dist < 1):
return (0, 0)
elif (abs(goal_left / goal_dist) > float(self.CAMERA_SIDE) /
self.CAMERA_DEPTH): # Is the goal within viewing limits?
return (0, 0)
else:
return (goal_dist, goal_left)
def shoot(self):
"If there is a grabbed ball, the ball is shot off"
with self.grabbed_ball_lock:
if self.grabbed_ball is not None:
self.grabbed_ball.center = self.center + self.forward * (
self.grabbed_forward + 10) + self.left * self.grabbed_left
# Shoots the ball at 0.4 pixels per millisecond (2.0 m/s)
self.grabbed_ball.v = self.forward * 0.4
self.grabbed_ball = None
def camera(self):
"This is the 'camera' sensor. If any ball is found in the 'camera triangle', the distance and bearing to it are reported (with a random 10% noise)"
"If several balls are found, the closest is reported"
v_forward_closest = 5000
v_left_closest = 5000
for b in self.world.objects:
if isinstance(b, Ball):
# First check distance to center
v = b.center - self.center
v_forward = self.forward.inner_product(v)
if v_forward > 0 and v_forward < self.CAMERA_DEPTH:
# See whether the ball is within the triangle
v_left = self.left.inner_product(v)
tan = abs(v_left) / v_forward
if (tan < float(self.CAMERA_SIDE) / self.CAMERA_DEPTH):
# The ball is inside
if (v_forward < v_forward_closest):
v_forward_closest = v_forward
v_left_closest = v_left
if (v_forward_closest != 5000):
return (v_forward_closest * random.uniform(0.9, 1.1),
v_left_closest * random.uniform(0.9, 1.1))
return None
class Robot(WorldObject): # Robot must fit into a 350mm cylinder, which here means that it should not exceed a square of 49x49 pixels more or less. Hence the width/height parameters. def __init__(self, world, name, role): width = 40 height = 40 center = Point(int(12+height/2), int(12+width/2)) if role == "TOPLEFT" else Point(world.width-(12+height/2), world.height-(12+width/2)) WorldObject.__init__(self, center, int(sqrt(width**2 + height**2)/2) ) self.world = world self.name = name self.wr = width/2 self.hr = height/2 self.beacon_point = Point(world.width + 50, world.cy) if role == "TOPLEFT" else Point(-50, world.cy) self.beacon_point_en = Point(0, world.cy) if role == "TOPLEFT" else Point(world.width, world.cy) self.goal_center = Point(world.width, world.cy) if role == "TOPLEFT" else Point(0, world.cy) # "Forward" is the unit direction vector where the robot is facing # (0, 1) is the "default" (height along the Y axis) self.forward = Point(0, 1) self.left = Point(-self.forward.y, self.forward.x) if role == "TOPLEFT": self.rotate(3.1415/2) else: self.rotate(-3.1415/2) # Those are state parameters self.leftSpeed = 0 self.rightSpeed = 0 # Camera sensor parameters self.CAMERA_DEPTH = 800 # Kaugus kuhu naeme self.CAMERA_SIDE = 500 # Laius kuhu naeme # Write concurrency lock (we assume reads are atomic, and even if not, just ignore read errors) self.data_lock = thread.allocate_lock() # Whether there's a ball in the grabber self.grabbed_ball = None self.grabbed_ball_lock = thread.allocate_lock() def draw(self, screen): # Center point draw.line(screen, (0,0,0), (self.center - self.left*5).as_tuple(), (self.center + self.left*5).as_tuple()) draw.line(screen, (0,0,0), self.center.as_tuple(), (self.center + self.forward*8).as_tuple()) # Sensor edges draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) draw.line(screen, (255,0,0), self.center.as_tuple(), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple()) draw.line(screen, (255,0,0), (self.center + self.forward*self.CAMERA_DEPTH + self.left*(-self.CAMERA_SIDE)).as_tuple(), \ (self.center + self.forward*self.CAMERA_DEPTH + self.left*self.CAMERA_SIDE).as_tuple()) # Beacon line if (self.beacon()): draw.line(screen, (255, 100, 0), self.center.as_tuple(), self.beacon_point.as_tuple()) # Wheels for side in [1, -1]: draw.line(screen, (0,0,0), (self.center + self.left*self.wr*side - self.forward*8).as_tuple(), (self.center + self.left*self.wr*side + self.forward*8).as_tuple(), 5) # Sides for (f,t) in self.edges(): draw.line(screen, (0,0,0), f.as_tuple(), t.as_tuple()) def edges(self): """Enumerate edges as tuples ((x,y), (x,y)) in clockwise order [assuming mathematical coordinates]""" leftback = self.center + self.left*self.wr - self.forward*self.hr leftfront = self.center + self.left*self.wr + self.forward*self.hr rightfront = self.center - self.left*self.wr + self.forward*self.hr rightback = self.center - self.left*self.wr - self.forward*self.hr yield (leftback, leftfront) yield (leftfront, rightfront) yield (rightfront, rightback) yield (rightback, leftback) def rotate(self, angle): self.forward.rotate(angle) self.left.rotate(angle) def simulate(self): # This is a hack which only works at small simulation steps leftTurn = (self.leftSpeed - self.rightSpeed)/self.wr/2 forwardMove = (self.leftSpeed + self.rightSpeed)/2 self.v = self.forward*forwardMove if (self.v != 0): self.center.add(self.v) if (leftTurn != 0): self.forward = self.forward + self.left*leftTurn self.forward.normalize() self.left = Point(-self.forward.y, self.forward.x) # If there is a grabbed ball, carry it around with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.v = Point(0,0) self.grabbed_ball.center = self.center + self.forward*self.grabbed_forward + self.left*self.grabbed_left # Precompute "edge walls", those will be useful in collision checks self.edge_walls = [Wall(e[1], e[0]) for e in self.edges()] all_x = [w.p1.x for w in self.edge_walls] all_y = [w.p1.y for w in self.edge_walls] def wall_check(self, w): # Robot's wall handling is fairly trivial. If we see we're hitting the wall, we'll nudge back from it m = min([w.dist_to_point(edge[0]) for edge in self.edges()]) if (m < 0): # We need to nudge perpendicular to the wall by distance -m self.center.add(w.normal*(-m)) def collision_check(self, obj): # If it is not a ball, ignore it if not isinstance(obj, Ball): # objekt ei ole pall # Just check the "bounding circle" dir = obj.center - self.center dist = dir.norm() if (dist < obj.radius + self.radius): # Nudge either us or them, choose randomly to avoid some ugliness dir_normalized = dir * (1/dist) if (random.randint(0,1) == 0): # Them obj.center.add(dir_normalized*(obj.radius + self.radius - dist)) else: # Us self.center.add(dir_normalized*(dist - (obj.radius + self.radius))) else: #objekt on pall # Find which wall is the ball touching for w in self.edge_walls: d = w.dist_to_point(obj.center) - obj.radius if (d > -0.3 and d < 0): # Is the ball within the range of the wall at all? wall_coord = (obj.center - w.p1).inner_product(w.v_normalized) if (wall_coord >= -obj.radius and wall_coord <= w.len+obj.radius): # Yes, it does, something must be done. # Nyyd kontrollime kas esimene ots? v = obj.center - self.center v_left = self.left.inner_product(v) v_forward = self.forward.inner_product(v) if (abs(v_left) < self.wr - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = obj obj.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return else: #kui pole esimene ots # First, nudge obj.center.add(w.normal * (-d)) # Second, simulate a rebounce (this is a hack, but it's way easier than considering rotations and stuff) obj.v.add(w.normal * (-d*2)) # ----------- The following are the main commands for the robot ----------------- def wheels(self, left, right): """ Sets the wheel speed. Left and right must be numbers -100..100. 100 is 1 m/s. """ with self.data_lock: self.leftSpeed = left /500.0 # leftSpeed and rightSpeed are in pixels per second self.rightSpeed = right /500.0 def grab(self): "If at the moment this function is called there is a ball right at the front of the robot, the ball is 'grabbed'" if self.grabbed_ball is not None: #pall on juba haaratud, meil peaks olema, et votab koik pallid return for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.hr + b.radius + 3: # See whether the ball is within the front edge v_left = self.left.inner_product(v) if (abs(v_left) < self.wr - 2): # OK, grab with self.grabbed_ball_lock: self.grabbed_ball = b b.v = Point(0, 0) self.grabbed_forward = v_forward - 5 self.grabbed_left = v_left return def beacon(self): "Returns true if cos(angle) to beacon is > 0.99" dbeacon = self.beacon_point - self.center dbeacon.normalize() #originaalis oli 0.99 mis vastab 8.1 kraadile meie kasutame 0.995 mis vastab 5.7le, 993-6.7kraadi return dbeacon.inner_product(self.forward) > 0.994 def shoot(self): "If there is a grabbed ball, the ball is shot off" with self.grabbed_ball_lock: if self.grabbed_ball is not None: self.grabbed_ball.center = self.center + self.forward*(self.grabbed_forward + 10) + self.left*self.grabbed_left # Shoots the ball at 0.4 pixels per millisecond (2.0 m/s) self.grabbed_ball.v = self.forward * 1 ## oli 0.4 mis vastab 2 ms, paneme 2 mis vastab 10m/s self.grabbed_ball = None def camera(self): "This is the 'camera' sensor. If any ball is found in the 'camera triangle', the distance and bearing to it are reported (with a random 10% noise)" "If several balls are found, one of them is reported (typically it is a stable solution)" v_forward_closest = 5000; v_left_closest = 5000; for b in self.world.objects: if isinstance(b, Ball): # First check distance to center v = b.center - self.center v_forward = self.forward.inner_product(v) if v_forward > 0 and v_forward < self.CAMERA_DEPTH: # See whether the ball is within the triangle v_left = self.left.inner_product(v) tan = abs(v_left)/v_forward if (tan < float(self.CAMERA_SIDE)/self.CAMERA_DEPTH): # The ball is inside if (v_forward < v_forward_closest): v_forward_closest = v_forward v_left_closest = v_left if (v_forward_closest != 5000): return (v_forward_closest*random.uniform(0.9,1.1), v_left_closest*random.uniform(0.9,1.1)) return None def optokatkesti(self): "See on optokatkesti kontrollimine. Kui pall on triblajas, siis peab vastama 1, kui ei ole siis False" if self.grabbed_ball is not None: #Pall on haaratud return 1