def run(self):
frameCounter = 0
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles({
'x': 0.75,
'y': 0
}, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
self.left_motor.setVelocity(1)
self.right_motor.setVelocity(-1)
def run(self):
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
if direction == 0:
left_speed = 5
right_speed = 5
else:
left_speed = direction * 4
right_speed = direction * -4
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
def run(self):
if self.name[0] == "B":
goalDir = -1
else:
goalDir = 1
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
goToPos = utils.calculateGBRLine(ball_pos, goalDir)
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(goToPos, robot_pos)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 8
right_speed = direction * -8
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
def moveTo(x, y):
robot_angle_2 = robot_pos['orientation']
angle = math.atan2(
y - robot_pos['y'],
x - robot_pos['x'],
)
if angle < 0:
angle = 2 * math.pi + angle
if robot_angle_2 < 0:
robot_angle_2 = 2 * math.pi + robot_angle_2
angle2 = math.degrees(angle + robot_angle_2)
angle2 -= 90
if angle2 > 360:
angle2 -= 360
d2 = utils.get_direction(angle2)
return d2
def run(self):
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
#if direction == 0:
# left_speed = -5
# right_speed = -5
#else:
# left_speed = direction * 4
# right_speed = direction * -4
real_robot_angle = robot_angle*57
robot_x = robot_pos["x"]*100
robot_y = robot_pos["y"]*100
ball_x = ball_pos["x"]*100
ball_y = ball_pos["y"]*100
brana_x = -75
def get_angles(pos1_x, pos1_y):
if polovica == 1:
pos2_x = -75
else:
pos2_x = 75
pos2_y = 0
if pos1_y < 0:
pos1_y = pos1_y * (-1)
if pos1_x < 0:
pos1_x = pos1_x * (-1)
a = pos2_x - pos1_x
b = pos2_y - pos1_y
c = math.sqrt(a**2 + b**2)
global alfa
alfa = math.degrees(math.asin(a / c))
beta = math.degrees(math.asin(b / c))
gama = 90
return beta
def zisti_ci_mas_loptu():
if polovica == 1:
if robot_x < ball_x and robot_x + 5 > ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y:
return 1
else:
return 0
else:
if robot_x > ball_x and robot_x - 5 < ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y:
return 1
else:
return 0
def navigacia():
if robot_y > 0:
cielovy_uhol = 90 + get_angles(robot_x, robot_y)
else:
cielovy_uhol = 90 - get_angles(robot_x, robot_y)
global vr, vl
if real_robot_angle + 4 >= cielovy_uhol and real_robot_angle - 4 <= cielovy_uhol:
vr = 10
vl = 10
else:
if real_robot_angle < cielovy_uhol:
vr = -5
vl = 5
else:
vr = 5
vl = -5
global a
if a == 0:
if robot_x < 0:
polovica = -1
else:
polovica = 1
a = 1
if polovica == 1:
if robot_x < 20:
if zisti_ci_mas_loptu() == 0:
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 10
right_speed = direction * -10
else:
print("mamy2")
if ball_y > -10 and ball_y < 10:
navigacia()
left_speed = vl
right_speed = vr
else:
if ball_y < 0:
left_speed = -5
right_speed = -10
else:
left_speed = -10
right_speed = -5
else:
if real_robot_angle <= 274:
if real_robot_angle >= 264:
left_speed = -10
right_speed = -10
else:
left_speed = 3
right_speed = -3
else:
left_speed = -3
right_speed = 3
else:
if robot_x > -25:
if zisti_ci_mas_loptu() == 0:
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 10
right_speed = direction * -10
else:
if ball_y > -10 and ball_y < 10:
navigacia()
left_speed = vl
right_speed = vr
else:
if ball_y < 0:
left_speed = -5
right_speed = -10
else:
left_speed = -10
right_speed = -5
else:
if real_robot_angle <= 274:
if real_robot_angle >= 264:
left_speed = 10
right_speed = 10
else:
left_speed = 3
right_speed = -3
else:
left_speed = -3
right_speed = 3
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
def evanMethod(self):
moving = True
shooting = False
ball_moving = False
ball_pos_last = [0, 0]
waiting_for_ball = False
team = -1
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.name[0] == 'B':
team = 1
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
if not (ball_pos['x'] == ball_pos_last[0]) and not (
ball_pos['y'] == ball_pos_last[0]):
ball_moving = True
ball_change_x = ball_pos['x'] - ball_pos_last[0]
ball_change_y = ball_pos['y'] - ball_pos_last[1]
# print(ball_change_x, ball_change_y)
robot_angle_2 = robot_pos['orientation']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
rx = robot_pos['x']
ry = robot_pos['y']
bx = ball_pos['x']
by = ball_pos['y']
if team == 1:
ball_x_dist_from_goal = abs(bx + 0.75)
else:
ball_x_dist_from_goal = bx - 0.75
ball_y_dist_from_goal = by
if team == 1:
if (rx - 0.05 < bx):
moving = True
shooting = False
elif (rx + 0.05 > bx):
moving = True
shooting = False
# print("byd"+str(ball_y_dist_from_goal))
# shotx = bx + 0.15/math.sqrt(ball_x_dist_from_goal**2+ball_y_dist_from_goal**2)*ball_x_dist_from_goal + 20*ball_change_x
shotx = bx + 0.2 / math.sqrt(
ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2
) * ball_x_dist_from_goal + 10 * ball_change_x
shoty = by + 0.2 / math.sqrt(
ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2
) * ball_y_dist_from_goal + 23 * ball_change_y
if shoty > 0.65:
shoty = shoty = by + 0.2 / math.sqrt(
ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2
) * ball_y_dist_from_goal + 23 * ball_change_y
if shoty > 0.65:
shoty = 0.65
if shoty < -0.65:
shoty = -0.65
if shotx > 0.65:
shotx = 0.60
if shotx < -0.65:
shotx = -0.60
# print(ball_x_dist_from_goal)
# print(shotx, shoty)
#if not on wall or not behind robot and compensate for movement, improve shooting mechanism (focusing on center of goal instead of ball)
xtarget = shotx
ytarget = shoty
if team == 1:
if xtarget > 0.2:
xtarget = 0.2
ytarget = 0
else:
if xtarget < -0.2:
xtarget = -0.2
ytarget = 0
ball_pos = data['ball']
xb = ball_pos['x']
yb = ball_pos['y']
b1 = data[self.name]
bX = b1['x']
bY = b1['y']
bdist = math.sqrt((bX - xb)**2 + (bY - yb)**2)
# if bdist < 0.1:
# xtarget = 0.2
# ytarget = 0
# TO KEEP THE ATTACKER ON OFFENSE
# if (xtarget > 0.2 and bdist > 0.1) or xtarget > 0.4:
# xtarget = 0.12
# ytarget = 0
#
def moveTo(x, y):
robot_angle_2 = robot_pos['orientation']
angle = math.atan2(
y - robot_pos['y'],
x - robot_pos['x'],
)
if angle < 0:
angle = 2 * math.pi + angle
if robot_angle_2 < 0:
robot_angle_2 = 2 * math.pi + robot_angle_2
angle2 = math.degrees(angle + robot_angle_2)
angle2 -= 90
if angle2 > 360:
angle2 -= 360
d2 = utils.get_direction(angle2)
return d2
if moving:
sp = moveTo(xtarget, ytarget)
if abs(xtarget - rx) < 0.02 and abs(ytarget - ry) < 0.02:
moving = False
# print("DONE")
shooting = True
direction = sp
if shooting:
if team == 1:
angle = math.atan2(
-robot_pos['y'],
-0.75 - robot_pos['x'],
)
else:
angle = math.atan2(
-robot_pos['y'],
0.75 - robot_pos['x'],
)
if angle < 0:
angle = 2 * math.pi + angle
if robot_angle_2 < 0:
robot_angle_2 = 2 * math.pi + robot_angle_2
angle2 = math.degrees(angle + robot_angle_2)
angle2 -= 90
if angle2 > 360:
angle2 -= 360
direction = utils.get_direction(angle2)
if direction == 0:
left_speed = -10
right_speed = -10
else:
right_speed = direction * -10
left_speed = direction * 10
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
ball_pos_last = [ball_pos['x'], ball_pos['y']]
def run(self):
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
#if direction == 0:
# left_speed = -5
# right_speed = -5
#else:
# left_speed = direction * 4
# right_speed = direction * -4
real_robot_angle = robot_angle * 57
robot_x = robot_pos["x"] * 100
robot_y = robot_pos["y"] * 100
ball_x = ball_pos["x"] * 100
ball_y = ball_pos["y"] * 100
#superova brana je x 70
#a y je 15 az -15
def buduce_suradnice_lopty():
global buduceX, buduceY, loptaX, loptaY, koeficient
if len(loptaX) == 2:
loptaX.pop(0)
loptaY.pop(0)
loptaX.append(ball_x)
loptaY.append(ball_y)
buduceX = loptaX[1] + (
(loptaX[1] - loptaX[0]) * koeficient)
buduceY = loptaY[1] + (
(loptaY[1] - loptaY[0]) * koeficient)
else:
loptaX.append(ball_x)
loptaY.append(ball_y)
def get_angles(pos1_x, pos1_y):
if polovica == 1:
pos2_x = -75
else:
pos2_x = 75
pos2_y = 0
if pos1_y < 0:
pos1_y = pos1_y * (-1)
if pos1_x < 0:
pos1_x = pos1_x * (-1)
a = pos2_x - pos1_x
b = pos2_y - pos1_y
c = math.sqrt(a**2 + b**2)
global alfa
alfa = math.degrees(math.asin(a / c))
beta = math.degrees(math.asin(b / c))
gama = 90
return beta
def zisti_ci_mas_loptu():
if polovica == -1:
if robot_x < ball_x and robot_x + 5 > ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y:
return 1
else:
return 0
else:
if robot_x > ball_x and robot_x - 5 < ball_x and robot_y + 4 > ball_y and robot_y - 4 < ball_y:
return 1
else:
return 0
def navigacia():
if robot_y > 0:
cielovy_uhol = 90 + get_angles(robot_x, robot_y)
else:
cielovy_uhol = 90 - get_angles(robot_x, robot_y)
global vr, vl
if real_robot_angle + 4 >= cielovy_uhol and real_robot_angle - 4 <= cielovy_uhol:
vr = 10
vl = 10
else:
if real_robot_angle < cielovy_uhol:
vr = -3
vl = 3
else:
vr = 3
vl = -3
global a
if a == 0:
if robot_x < 0:
polovica = -1
else:
polovica = 1
start_time = time.time()
a = 1
if ball_x > 75 or ball_x < -75:
a = 0
if (time.time() - start_time) < 4:
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 5
right_speed = direction * -5
else:
if polovica == 1:
if robot_x < 20:
if zisti_ci_mas_loptu() == 0:
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 10
right_speed = direction * -10
else:
print("mamb3")
if ball_y > -10 and ball_y < 10:
navigacia()
left_speed = vl
right_speed = vr
else:
if ball_y < 0:
left_speed = -5
right_speed = -10
else:
left_speed = -10
right_speed = -5
else:
if real_robot_angle <= 274:
if real_robot_angle >= 264:
left_speed = -10
right_speed = -10
else:
left_speed = 3
right_speed = -3
else:
left_speed = -3
right_speed = 3
else:
if robot_x > -25:
if zisti_ci_mas_loptu() == 0:
if direction == 0:
left_speed = -10
right_speed = -10
else:
left_speed = direction * 10
right_speed = direction * -10
else:
if ball_y > -10 and ball_y < 10:
navigacia()
left_speed = vl
right_speed = vr
else:
if ball_y < 0:
left_speed = -5
right_speed = -10
else:
left_speed = -10
right_speed = -5
else:
if real_robot_angle <= 274:
if real_robot_angle >= 264:
left_speed = 10
right_speed = 10
else:
left_speed = 3
right_speed = -3
else:
left_speed = -3
right_speed = 3
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
def run(self):
frameCounter = 0
if self.name[0] == 'Y':
t_m = 1
else:
t_m = -1
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# # Compute the speed for motors
direction = utils.get_direction(ball_angle)
#
# # If the robot has the ball right in front of it, go forward,
# # rotate otherwise
# # 0 forward, -1 right, 1 left
# if direction == 0:
# left_speed = -5
# right_speed = -5
# else:
# left_speed = direction * 4
# right_speed = direction * -4
status = ""
target_x = -0.07
idle_speed = 4
if robot_pos['y'] > -0.03 and robot_pos['y'] < 0.03:
if ball_pos['y'] > -0.05 and ball_pos[
'y'] < 0.05 and robot_pos['x'] * t_m < ball_pos[
'x'] * t_m:
if direction == 0:
left_speed = -10
right_speed = -10
status = "charging"
else:
left_speed = direction * 4
right_speed = direction * -4
status = "aiming"
else:
if ball_pos['y'] < 0.3 and ball_pos[
'y'] > -0.3 and robot_pos[
'x'] * t_m < ball_pos['x'] * t_m:
target_x = ball_pos['x'] - 0.08
idle_speed = 10 * t_m
if robot_angle < ((math.pi / 2) - 0.05):
left_speed = 2
right_speed = -2
status = "turning"
elif robot_angle > ((math.pi / 2) + 0.05):
left_speed = -2
right_speed = 2
status = "turning"
else:
if robot_pos['x'] * t_m < target_x:
left_speed = -idle_speed * t_m
right_speed = -idle_speed * t_m
else:
left_speed = idle_speed * t_m
right_speed = idle_speed * t_m
status = "idling"
else:
if robot_angle < (math.pi - 1):
left_speed = 0
right_speed = 0
status = "turning back"
elif robot_angle > (math.pi + 1):
left_speed = 0
right_speed = 0
status = "turning back"
else:
left_speed = -5
right_speed = -5
status = "returning"
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
def evanMethod(self, ball_pos_last):
moving = True
shooting = False
ball_moving = False
waiting_for_ball = False
xbOLD = 0
GETOUT = False
team = -1
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.name[0] == 'B':
team = 1
if self.is_new_data():
data = self.get_new_data()
# Get the position of the ball
ball_pos = data['ball']
xb = ball_pos['x']
if team == 1:
if xb > 0.3:
GETOUT = False
else:
GETOUT = True
else:
if xb < -0.3:
GETOUT = False
else:
GETOUT = True
xbOLD = xb
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
if not (ball_pos['x'] == ball_pos_last[0]) and not (
ball_pos['y'] == ball_pos_last[0]):
ball_moving = True
print("bl" + str(ball_pos_last[1]))
ball_change_x = ball_pos['x'] - ball_pos_last[0]
ball_change_y = ball_pos['y'] - ball_pos_last[1]
# print(ball_change_x, ball_change_y)
print("ball_change_y: " + str(ball_change_y))
# for x in range(10):
robot_angle_2 = robot_pos['orientation']
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(ball_pos, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
rx = robot_pos['x']
ry = robot_pos['y']
bx = ball_pos['x']
by = ball_pos['y']
if team == 1:
ball_x_dist_from_goal = abs(bx + 0.75)
else:
ball_x_dist_from_goal = bx - 0.75
ball_y_dist_from_goal = by
if team == 1:
if (rx - 0.05 < bx):
moving = True
shooting = False
elif (rx + 0.05 > bx):
moving = True
shooting = False
# print("byd"+str(ball_y_dist_from_goal))
# shotx = bx + 0.15/math.sqrt(ball_x_dist_from_goal**2+ball_y_dist_from_goal**2)*ball_x_dist_from_goal + 20*ball_change_x
shotx = bx
shoty = by
if shoty > -0.25 or shoty < 0.25 and shotx > 0.70 and shotx < -0.70:
shoty = shoty = by + 0.2 / math.sqrt(
ball_x_dist_from_goal**2 + ball_y_dist_from_goal**2
) * ball_y_dist_from_goal + 23 * ball_change_y
if shoty > 0.33:
shoty = 0.33
if shoty < -0.33:
shoty = -0.33
if shotx > 0.70:
shotx = 0.75
if shotx < -0.70:
shotx = -0.75
print("shoty: " + str(shoty))
print("by" + str(by))
print("ry" + str(ry))
# print(ball_x_dist_from_goal)
# print(shotx, shoty)
#if not on wall or not behind robot and compensate for movement, improve shooting mechanism (focusing on center of goal instead of ball)
xtarget = shotx
ytarget = shoty
def moveTo(x, y):
robot_angle_2 = robot_pos['orientation']
angle = math.atan2(
y - robot_pos['y'],
x - robot_pos['x'],
)
if angle < 0:
angle = 2 * math.pi + angle
if robot_angle_2 < 0:
robot_angle_2 = 2 * math.pi + robot_angle_2
angle2 = math.degrees(angle + robot_angle_2)
angle2 -= 90
if angle2 > 360:
angle2 -= 360
d2 = utils.get_direction(angle2)
return d2
if moving:
sp = moveTo(xtarget, ytarget)
direction = sp
if direction == 0:
left_speed = -10
right_speed = -10
else:
right_speed = direction * -10
left_speed = direction * 10
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
ball_pos_last = [ball_pos['x'], ball_pos['y']]
if GETOUT:
break
def run(self):
BLOCK = False
SPOTONE = True
SPOTTWO = False
ROAMCLOSE = False
xbOLD = 0
ybOLD = 0
ball_pos_last = [0, 0]
#worth it to take the penalty
spotY = 0.02
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
if self.name[0] == 'B':
team = 1
else:
team = -1
if team == 1:
spotX = 0.55
else:
spotX = -0.55
# Get the position of our robot
robot_pos = data[self.name]
orientation = robot_pos['orientation']
xr = robot_pos['x']
yr = robot_pos['y']
# Get the position of the ball
ball_pos = data['ball']
xb = ball_pos['x']
yb = ball_pos['y']
b1 = data['B1']
b3 = data['B3']
if abs(xr - spotX) <= 0.05 and abs(yr - spotY) <= 0.05:
SPOTONE = False
if abs(xr - spotX) <= 0.05 and abs(yr + spotY) <= 0.05:
SPOTONE = True
if team == 1:
if xb > 0.3:
BLOCK = True
else:
BLOCK = False
else:
if xb < -0.3:
BLOCK = True
else:
BLOCK = False
if BLOCK:
self.evanMethod(ball_pos_last)
elif SPOTONE:
robotPointAngle, robot_angle = utils.getPointAngle(
orientation, xr, yr, spotX, spotY)
direction = utils.get_direction(robotPointAngle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
if direction == 0:
left_speed = -10
right_speed = -10
elif direction == -1:
left_speed = direction * 10
right_speed = direction * -10
else:
left_speed = direction * 10
right_speed = direction * -10
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
else:
robotPointAngle, robot_angle = utils.getPointAngle(
orientation, xr, yr, spotX, spotY * -1)
direction = utils.get_direction(robotPointAngle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
if direction == 0:
left_speed = -10
right_speed = -10
elif direction == -1:
left_speed = direction * 10
right_speed = direction * -10
else:
left_speed = direction * 10
right_speed = direction * -10
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
xbOLD = xb
yBOLD = yb
ball_pos_last = [ball_pos['x'], ball_pos['y']]
def run(self):
frameCounter = 0
past_position = {"x": 0, "y": 0}
x_diff = 1
y_diff = 1
slope = 0
if self.name[0] == 'Y':
t_m = 1
else:
t_m = -1
status = "idling"
past_intercept = {"x": 0, "y": 0}
while self.robot.step(rcj_soccer_robot.TIME_STEP) != -1:
if self.is_new_data():
data = self.get_new_data()
# Get the position of our robot
robot_pos = data[self.name]
# Get the position of the ball
ball_pos = data['ball']
x_diff = ball_pos['x'] - past_position['x']
y_diff = ball_pos['y'] - past_position['y']
# if ball_pos['x'] > -0.75 and ball_pos['x'] < -0.59 and ball_pos['y'] > -0.35 and ball_pos['y'] < 0.35:
if frameCounter % 15 == 0:
if x_diff != 0:
slope = y_diff / x_diff
to_go = self.get_next_position(slope, ball_pos,
robot_pos, t_m)
if to_go == {}:
status = "idling"
if robot_pos['y'] < 0.1:
to_go = {'x': -0.56 * t_m, 'y': 0.11}
elif robot_pos['y'] > 0.1:
to_go = {'x': -0.56 * t_m, 'y': -0.11}
elif ball_pos['x'] * t_m < -0.35 and robot_pos[
'x'] > to_go['x'] - 0.02 and robot_pos[
'x'] < to_go['x'] + 0.02 and robot_pos[
'y'] > to_go['y'] - 0.02 and robot_pos[
'y'] < to_go['y'] + 0.02 and abs(
to_go['y'] != 0.11):
to_go = ball_pos
status = "charging"
# print("charging")
elif status == "charging":
if past_intercept['y'] > to_go[
'y'] - 0.01 and past_intercept['y'] < to_go[
'y'] + 0.01 and past_intercept[
'x'] > to_go[
'x'] - 0.01 and past_intercept[
'x'] < to_go['x'] + 0.01:
to_go = ball_pos
status = "charging"
else:
status = "idling"
else:
to_go = self.track_ball(ball_pos, t_m)
status = "idling"
if to_go == {}:
if robot_pos['y'] < 0.1:
to_go = {'x': -0.56 * t_m, 'y': 0.11}
elif robot_pos['y'] > 0.1:
to_go = {'x': -0.56 * t_m, 'y': -0.11}
past_position = ball_pos
past_intercept = to_go
# Get angle between the robot and the ball
# and between the robot and the north
ball_angle, robot_angle = self.get_angles(to_go, robot_pos)
# Compute the speed for motors
direction = utils.get_direction(ball_angle)
# If the robot has the ball right in front of it, go forward,
# rotate otherwise
# normal forward speed is 5, but i turned it up to 10 - stanley
if direction == 0:
left_speed = -10
right_speed = -10
elif direction == 2:
left_speed = 10
right_speed = 10
else:
left_speed = direction * 4
right_speed = direction * -4
# Set the speed to motors
self.left_motor.setVelocity(left_speed)
self.right_motor.setVelocity(right_speed)
frameCounter += 1
