# 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

"""

This is the robot's network controller interface. It accepts commands over TCP and forwards them to the robot.

Usage:

r = Robot( ... ) # create the robot instance

s = RobotServer(r, port=5000) # create the robot server

s.serve() # starts a new thread with the server. The thread runs forever.

"""

def __init__(self, robot, port=5000):

self.robot = robot

self.port = port

def serve(self):

"""Starts the server in a separate thread"""

import thread

thread.start_new_thread(self._server_thread, tuple())

def _server_thread(self):

# Echo server program

print "Starting server at port %d" % self.port

import socket

HOST = '' # Symbolic name meaning all available interfaces

s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) #Vabastamine pordi

s.bind((HOST, self.port))

s.listen(1)

print "Robot %s listening at port %d" % (self.robot.name, self.port)

while 1:

try:

conn, addr = s.accept()

print 'Connected by', addr

while 1:

data = conn.recv(1024)

if not data: break

#print "%s<< %s" % (self.robot.name, data.strip())

response = self._process_command(data)

#print "%s>> %s" % (self.robot.name, response.strip())

conn.send(response + "\n")

conn.close()

except:

traceback.print_exc()

def _process_command(self, cmd):

"Reaction to each command"

try:

c = cmd.split()

if c[0] == "WHEELS":

l,r = int(c[1]), int(c[2])

if (l < -100 or l > 100 or r < -100 or r > 100):

return "ERROR"

# Introduce up to 10% error in settings

ltrue = l*random.uniform(0.9, 1.1)

rtrue = r*random.uniform(0.9, 1.1)

self.robot.wheels(ltrue, rtrue)

return "OK"

elif c[0] == "CAM":

c = self.robot.camera()

if c is None:

return "0 0"

else:

return "%f %f" % c

elif c[0] == "GRAB":

self.robot.grab()

return "OK"

elif c[0] == "SHOOT":

self.robot.shoot()

return "OK"

elif c[0] == "BEACON":

return "1" if self.robot.beacon() else "0"

elif c[0] == "OPTO": #Lisatud optokatkesti

return "1" if self.robot.optokatkesti() else "0"

else:

return "ERROR: else"

except: