class TeamK:
def __init__(self, field, goal, penalty_area, goal_area, robot_size,
max_linear_velocity):
self.field = field
self.goal = goal
self.penalty_area = penalty_area
self.goal_area = goal_area
self.robot_size = robot_size
self.max_linear_velocity = max_linear_velocity
self.gk_index = 0
self.d1_index = 1 #1
self.d2_index = 2 #2
self.f1_index = 3 #3
self.f2_index = 4 #4
self.pass_count = 0
self.cross_count = 0
self.shoot_count = 0
self.gk_target_robot_id = self.gk_index
self.d1_target_robot_id = self.d1_index
self.d2_target_robot_id = self.d2_index
self.f1_target_robot_id = self.f1_index
self.f2_target_robot_id = self.f2_index
self.action = ActionControl(max_linear_velocity)
self.robot_height = 0.421
self.flag = 0
def reset_counter(self):
self.pass_count = 0
self.cross_count = 0
self.shoot_count = 0
def move(self, idx, idx_opp, defense_angle, attack_angle, cur_posture,
cur_posture_opp, prev_posture, prev_posture_opp, prev_ball,
cur_ball, predicted_ball, reset_reason, game_state):
self.action.update_state(cur_posture, prev_posture, cur_ball,
prev_ball, reset_reason)
# GK variables
gk_protection_radius = self.goal_area[Y] / 2 - 0.1
gk_protection_x = math.cos(
defense_angle) * gk_protection_radius - self.field[X] / 2
gk_protection_y = math.sin(defense_angle) * gk_protection_radius
# D1 variables
d1_protection_radius = self.goal_area[Y] / 2 - 0.1
d1_protection_x = math.cos(
defense_angle) * d1_protection_radius - self.field[X] / 2
d1_protection_y = math.sin(defense_angle) * d1_protection_radius
if game_state == STATE_DEFAULT:
# GK ZONE
if helper.ball_is_gk_zone(predicted_ball, self.field,
self.goal_area):
# GK
# gk_control = self.action.defend_ball(self.gk_index)
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
if gk_control == None:
if (cur_posture[self.gk_index][BALL_POSSESSION]):
gk_control = self.action.shoot_to(
self.gk_index, 0, 0, 10, 10)
else:
if -self.field[X] / 2 - 0.05 < cur_posture[
self.gk_index][X] < -self.field[
X] / 2 + 0.15 and -0.02 < cur_posture[
self.gk_index][Y] < 0.02:
gk_control = self.action.turn_to(
self.gk_index, 0, 0)
gk_control = self.action.go_to(
self.gk_index, self.field[X] / 2, 0)
else:
gk_control = self.action.go_to(
self.gk_index, self.field[X] / 2, 0)
# D1
d1_control = self.action.go_to(self.d1_index,
self.field[X] / 2, 0)
# D2
d2_control = self.action.go_to(self.d2_index,
self.field[X] / 2, 0)
# F1
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2, 0)
# F2
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
self.flag = 1
# D1 ZONE
elif helper.ball_is_d1_zone(predicted_ball, self.field,
self.penalty_area):
# GK
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
#gk_control = self.action.defend_ball(self.gk_index)
if gk_control == None:
if (cur_posture[self.gk_index][BALL_POSSESSION]):
gk_control = self.action.shoot_to(
self.gk_index, 0, 0, 10, 10)
else:
if -self.field[X] / 2 - 0.05 < cur_posture[
self.gk_index][X] < -self.field[
X] / 2 + 0.15 and -0.02 < cur_posture[
self.gk_index][Y] < 0.02:
gk_control = self.action.turn_to(
self.gk_index, 0, 0)
gk_control = self.action.go_to(
self.gk_index, self.field[X] / 2, 0)
else:
gk_control = self.action.go_to(
self.gk_index, self.field[X] / 2, 0)
# D1
if (self.d1_index == idx):
if (cur_posture[self.d1_index][BALL_POSSESSION]):
if self.pass_count == 0:
self.d1_target_robot_id = self.d2_index if (
cur_posture[self.d1_index][TH] > 0
) else self.f1_index
self.pass_count += 1
else:
self.pass_count += 1
d1_control = self.action.shoot_to(
self.d1_index, 0, 0, 10, 10)
else:
if (cur_posture[self.d1_index][X] > predicted_ball[X]):
d1_control = self.action.turn_to(
self.d1_index, 0, 0)
else:
#d1_min_x = -self.field[X]/2 + self.goal_area[X] + 0.1
d1_control = self.action.go_to(
self.d1_index, self.field[X] / 2, 0)
else:
d1_control = self.action.go_to(self.d1_index,
self.field[X] / 2, 0)
# D2
d2_control = self.action.go_to(self.d2_index,
self.field[X] / 2, 0)
# F1
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2, 0)
# F2
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
self.flag = 2 #2
# D2 ZONE
elif helper.ball_is_d2_zone(predicted_ball, self.field):
# GK
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
# gk_control = self.action.defend_ball(self.gk_index)
if gk_control == None:
if -self.field[X] / 2 - 0.05 < cur_posture[
self.gk_index][X] < -self.field[
X] / 2 + 0.15 and -0.02 < cur_posture[
self.gk_index][Y] < 0.02:
gk_control = self.action.turn_to(self.gk_index, 0, 0)
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
else:
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
# D1
d1_control = self.action.turn_to(self.d1_index, 0, 0)
# D2
if (cur_posture[self.d2_index][BALL_POSSESSION]):
if self.pass_count == 0:
self.d2_target_robot_id = self.f2_index if (
cur_posture[self.d1_index][TH] > 0
) else self.f1_index
self.pass_count += 1
else:
self.pass_count += 1
d2_control = self.action.shoot_to(self.d2_index, 0, 0, 10,
10)
else:
if (cur_posture[self.d2_index][X] > predicted_ball[X]):
d2_control = self.action.turn_to(self.d2_index, 0, 0)
else:
# d2_min_x = -self.field[X]/2 + self.goal_area[X] + 0.1
d2_control = self.action.go_to(self.d2_index,
self.field[X] / 2, 0)
# F1
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2 - 0.01, 0)
# F2
if cur_ball[X] < 0:
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
else:
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
self.flag = 3 #3
# F1 ZONE
elif helper.ball_is_f1_zone(predicted_ball, self.field):
# GK
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
#gk_control = self.action.defend_ball(self.gk_index)
if gk_control == None:
if -self.field[X] / 2 - 0.05 < cur_posture[
self.gk_index][X] < -self.field[
X] / 2 + 0.15 and -0.02 < cur_posture[
self.gk_index][Y] < 0.02:
gk_control = self.action.turn_to(self.gk_index, 0, 0)
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
else:
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
# D1
d1_control = self.action.go_to(self.d1_index,
self.field[X] / 2 - 0.01, 0)
# D2
d2_control = self.action.go_to(self.d2_index,
self.field[X] / 2, 0)
# F1
if (cur_posture[self.f1_index][BALL_POSSESSION]):
if self.cross_count == 0:
self.f1_target_robot_id = self.f2_index
self.cross_count += 1
else:
self.cross_count += 1
f1_control = self.action.shoot_to(self.f1_index, 0, 0, 10,
10)
if f1_control == None:
self.f1_target_robot_id = self.d2_index
f1_control = self.action.pass_to(
self.f1_index,
cur_posture[self.f1_target_robot_id][X],
cur_posture[self.f1_target_robot_id][Y])
else:
if (cur_posture[self.f1_index][X] > predicted_ball[X]):
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2, 0)
else:
#f1_min_x = -self.field[X]/2 + self.goal_area[X] + 0.1
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2, 0)
# F2
if cur_ball[X] < 0:
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
else:
f2_control = self.action.go_to(self.f2_index,
self.field[X] / 2, 0)
self.flag = 4 #4
# F2 ZONE
else:
# GK
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
#gk_control = self.action.defend_ball(self.gk_index)
if gk_control == None:
if -self.field[X] / 2 - 0.05 < cur_posture[
self.gk_index][X] < -self.field[
X] / 2 + 0.15 and -0.02 < cur_posture[
self.gk_index][Y] < 0.02:
gk_control = self.action.turn_to(self.gk_index, 0, 0)
else:
gk_control = self.action.go_to(self.gk_index,
self.field[X] / 2, 0)
# D1
d1_control = self.action.go_to(self.d1_index,
self.field[X] / 2, 0)
# D2
d2_control = self.action.go_to(self.d2_index,
self.field[X] / 2, 0)
# F1
f1_control = self.action.go_to(self.f1_index,
self.field[X] / 2, 0)
# F2
if (cur_posture[self.f2_index][BALL_POSSESSION]):
f2_control = self.action.shoot_to(self.f2_index, 0, 0, 10,
10)
else:
f2_control = self.action.turn_to(self.f2_index, 0, 0)
self.flag = 6 #6
if (self.pass_count > 20 or self.cross_count > 20
or self.shoot_count > 20):
self.reset_counter()
elif game_state == STATE_GOALKICK:
if helper.distance(cur_ball[X], cur_posture[self.gk_index][X],
cur_ball[Y],
cur_posture[self.gk_index][Y]) <= 0.2:
gk_control = [0, 0, 10, 8, 0, 0]
else:
gk_control = self.action.go_to(self.gk_index, cur_ball[X],
cur_ball[Y])
d1_control = [0, 0, 0, 0, 0, 0]
d2_control = [0, 0, 0, 0, 0, 0]
f1_control = [0, 0, 0, 0, 0, 0]
f2_control = [0, 0, 0, 0, 0, 0]
elif game_state == STATE_CORNERKICK:
gk_control = [0, 0, 0, 0, 0, 0]
d1_control = [0, 0, 0, 0, 0, 0]
d2_control = [0, 0, 0, 0, 0, 0]
f1_control = [0, 0, 0, 0, 0, 0]
if helper.distance(cur_ball[X], cur_posture[self.f2_index][X],
cur_ball[Y],
cur_posture[self.f2_index][Y]) <= 0.2:
f2_control = [0, 0, 7, 5, 0, 0]
else:
f2_control = self.action.go_to(self.f2_index, cur_ball[X],
cur_ball[Y])
elif game_state == STATE_KICKOFF:
gk_control = [0, 0, 0, 0, 0, 0]
d1_control = [0, 0, 0, 0, 0, 0]
d2_control = [0, 0, 0, 0, 0, 0]
f1_control = [0, 0, 0, 0, 0, 0]
if helper.distance(cur_ball[X], cur_posture[self.f2_index][X],
cur_ball[Y],
cur_posture[self.f2_index][Y]) <= 0.2:
f2_control = [0, 0, 5, 0, 0, 0]
else:
f2_control = self.action.go_to(self.f2_index, cur_ball[X],
cur_ball[Y])
elif game_state == STATE_PENALTYKICK:
gk_control = [0, 0, 0, 0, 0, 0]
d1_control = [0, 0, 0, 0, 0, 0]
d2_control = [0, 0, 0, 0, 0, 0]
f1_control = [0, 0, 0, 0, 0, 0]
if helper.distance(cur_ball[X], cur_posture[self.f2_index][X],
cur_ball[Y],
cur_posture[self.f2_index][Y]) <= 0.3:
f2_control = [1.5, 2.5, 0, 0, 0, 0]
else:
f2_control = self.action.go_to(self.f2_index, cur_ball[X],
cur_ball[Y])
else:
gk_control = [0, 0, 0, 0, 0, 0]
d1_control = [0, 0, 0, 0, 0, 0]
d2_control = [0, 0, 0, 0, 0, 0]
f1_control = [0, 0, 0, 0, 0, 0]
f2_control = [0, 0, 0, 0, 0, 0]
return gk_control + d1_control + d2_control + f1_control + f2_control
class Forward_2:
def __init__(self, field, goal, penalty_area, goal_area, robot_size,
max_linear_velocity):
self.field = field
self.goal = goal
self.penalty_area = penalty_area
self.goal_area = goal_area
self.robot_size = robot_size
self.max_linear_velocity = max_linear_velocity
self.action = ActionControl(max_linear_velocity)
self.flag = 0
def move(self,
robot_id,
idx,
idx_opp,
defense_angle,
attack_angle,
cur_posture,
cur_posture_opp,
prev_posture,
previous_ball,
cur_ball,
predicted_ball,
cross=False,
shoot=False,
quickpass=False,
jump=False,
dribble=False):
self.action.update_state(cur_posture, prev_posture, cur_ball,
previous_ball)
## 회전하기
dx = cur_ball[X] - previous_ball[X]
dy = cur_ball[Y] - previous_ball[Y] + 0.00000001 #zero division
pred_x = predicted_ball[X]
steps = (cur_posture[robot_id][Y] - cur_ball[Y]) / dy
#로봇 앞에 공 위치할 때
if (pred_x > cur_posture[robot_id][X]):
pred_dist = pred_x - cur_posture[robot_id][X]
# 예측된 공의 위치가 가까우면, 소유할 경우만 (가져도 회전하느라 공이 이동해버린다....)
if (pred_dist > 0.1 and pred_dist < 0.3
and steps < 10) and cur_posture[robot_id][BALL_POSSESSION]:
#상대팀 골대 방향 찾고 그쪽으로 회전
goal_angle = helper.direction_angle(self, robot_id,
self.field[X] / 2, 0,
cur_posture)
left_wheel, right_wheel = self.action.turn_to(
robot_id, goal_angle)
kick_speed, kick_angle = self.action.kick(
cross, shoot, quickpass)
jump_speed = self.action.jump(jump)
dribble_mode = self.action.dribble(dribble)
return left_wheel, right_wheel, kick_speed, kick_angle, jump_speed, dribble_mode
## 공이 내 진영 골 영역 or 패널티 영역에 있을 때
if helper.ball_is_own_goal(
predicted_ball, self.field,
self.goal_area) or helper.ball_is_own_penalty(
predicted_ball, self.field, self.penalty_area):
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 1
#근데 골 점유한 경우
if cur_posture[robot_id][BALL_POSSESSION]:
self.flag = 2
#크로스
#a=self.action.cross_to(self,robot_id,self.field[X]/2,0,cur_posture)
a = self.action.cross_to(robot_id, self.field[X] / 2, 0,
0.421) #0.421: robot_height
#크로스 실패시
if a == None:
shoot = True
self.flag = 3
## 현재 위치에서 shoot chance가 있을 때
elif (helper.shoot_chance(self, robot_id, cur_posture, cur_ball)):
x = predicted_ball[X]
y = predicted_ball[Y]
self.flag = 4
if cur_posture[robot_id][BALL_POSSESSION]:
try:
b = self.action.shoot_to(robot_id, self.field[X] / 2, 0)
self.flag = 5
except:
shoot = True
self.flag = 6
else:
if (cur_ball[X] > -0.3 * self.field[X] / 2):
self.flag = 7
# if the robot can push the ball toward opponent's side, do it
if (cur_posture[robot_id][X] < cur_ball[X] - 0.05):
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 8
if cur_posture[robot_id][BALL_POSSESSION]:
self.flag = 9
shoot = True
else:
# otherwise go behind the ball
if (abs(cur_ball[Y] - cur_posture[robot_id][Y]) > 0.3):
x = cur_ball[X] - 0.2
y = cur_ball[Y]
self.flag = 10
else:
x = cur_ball[X] - 0.2
y = cur_posture[robot_id][Y]
self.flag = 11
else:
#x = -0.1 * self.field[X] / 2
try: #조금씩 이동+패스
goalx = self.field[X] / 2
goaly = 0
robotx = cur_posture[robot_id][X]
roboty = cur_posture[robot_id][Y]
dx = (robotx + goalx) / 6
dy = (roboty + goaly) / 6
self.action.pass_to(robot_id, dx, dy)
#print(dx)
#print(dy)
x = dx
y = dy
self.flag = 12
except:
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 13
'''
if helper.ball_is_own_goal(predicted_ball, self.field, self.goal_area):
x = -0.5
y = -1
self.flag = 1
elif helper.ball_is_own_penalty(predicted_ball, self.field, self.penalty_area):
x = -0.5
y = -1
self.flag = 2
elif helper.ball_is_own_field(predicted_ball):
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 3
elif helper.ball_is_opp_goal(predicted_ball, self.field, self.goal_area):
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 4
if cur_posture[robot_id][BALL_POSSESSION]:
quickpass = True
self.flag = 5
elif helper.ball_is_opp_penalty(predicted_ball, self.field, self.penalty_area):
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 6
if cur_posture[robot_id][BALL_POSSESSION]:
cross = True
self.flag = 7
else:
x = cur_ball[X]
y = cur_ball[Y]
self.flag = 8
if cur_posture[robot_id][BALL_POSSESSION]:
shoot = True
self.flag = 9
'''
'''
# if the ball is coming toward the robot, seek for shoot chance
if (robot_id == idx and helper.ball_coming_toward_robot(robot_id, cur_posture, previous_ball, cur_ball)):
dx = cur_ball[X] - previous_ball[X]
dy = cur_ball[Y] - previous_ball[Y]
pred_x = predicted_ball[X]
steps = (cur_posture[robot_id][Y] - cur_ball[Y]) / dy
# if the ball will be located in front of the robot
if (pred_x > cur_posture[robot_id][X]):
pred_dist = pred_x - cur_posture[robot_id][X]
# if the predicted ball location is close enough
if (pred_dist > 0.1 and pred_dist < 0.3 and steps < 10):
# find the direction towards the opponent goal and look toward it
goal_angle = helper.direction_angle(self, robot_id, self.field[X] / 2, 0, cur_posture)
left_wheel, right_wheel = self.action.turn_to(robot_id, goal_angle)
kick_speed, kick_angle = self.action.kick(cross, shoot, quickpass)
jump_speed = self.action.jump(jump)
dribble_mode = self.action.dribble(dribble)
return left_wheel, right_wheel, kick_speed, kick_angle, jump_speed, dribble_mode
# if the robot can shoot from current position
if (robot_id == idx and helper.shoot_chance(self, robot_id, cur_posture, cur_ball)):
x = predicted_ball[X]
y = predicted_ball[Y]
if cur_posture[robot_id][BALL_POSSESSION]:
shoot = True
# if this forward is closer to the ball
elif (robot_id == idx):
if (cur_ball[X] > -0.3 * self.field[X] / 2):
# if the robot can push the ball toward opponent's side, do it
if (cur_posture[robot_id][X] < cur_ball[X] - 0.05):
x = cur_ball[X]
y = cur_ball[Y]
if cur_posture[robot_id][BALL_POSSESSION]:
shoot = True
else:
# otherwise go behind the ball
if (abs(cur_ball[Y] - cur_posture[robot_id][Y]) > 0.3):
x = cur_ball[X] - 0.2
y = cur_ball[Y]
else:
x = cur_ball[X] - 0.2
y = cur_posture[robot_id][Y]
else:
x = -0.1 * self.field[X] / 2
y = cur_ball[Y]
# if this forward is not closer to the ball
else:
if (cur_ball[X] > -0.3 * self.field[X] / 2):
x = cur_ball[X] - 0.25
y = cur_ball[Y] - 0.5
else:
# ball is on right side
if (cur_ball[Y] < 0):
x = -0.1 * self.field[X] / 2
y = min(cur_ball[Y] - 0.5, self.field[Y] / 2 - self.robot_size / 2)
# ball is on left side
else:
x = -0.1 * self.field[X] / 2
y = max(cur_ball[Y] - 0.5, -self.field[Y] / 2 + self.robot_size / 2)
'''
left_wheel, right_wheel = self.action.go_to(robot_id, x, y)
kick_speed, kick_angle = self.action.kick(cross, shoot, quickpass)
jump_speed = self.action.jump(jump)
dribble_mode = self.action.dribble(dribble)
return left_wheel, right_wheel, kick_speed, kick_angle, jump_speed, dribble_mode