def goalkeeper(self, id):
# self.set_target_position(id, 1, 1, 1.4, 5.0, 0.4, False)
# return
# default desired position
x = (-self.field[X] / 2) + (self.robot_size[id] / 2) + 0.05
y = max(min(self.cur_ball[Y], (self.goal[Y] / 2 - self.robot_size[id] / 2)),
-self.goal[Y] / 2 + self.robot_size[id] / 2)
# if the robot is inside the goal, try to get out
if (self.cur_posture[id][X] < -self.field[X] / 2):
if (self.cur_posture[id][Y] < 0):
self.set_target_position(id, x, self.cur_posture[id][Y] + 0.2, 1.4, 5.0, 0.4, False)
else:
self.set_target_position(id, x, self.cur_posture[id][Y] - 0.2, 1.4, 5.0, 0.4, False)
# if the goalkeeper is outside the penalty area
elif (not self.in_penalty_area(self.cur_posture[id], MY_TEAM)):
# return to the desired position
self.set_target_position(id, x, y, 1.4, 5.0, 0.4, True)
# if the goalkeeper is inside the penalty area
else:
# if the ball is inside the penalty area
if (self.in_penalty_area(self.cur_ball, MY_TEAM)):
# if the ball is behind the goalkeeper
if (self.cur_ball[X] < self.cur_posture[id][X]):
# if the ball is not blocking the goalkeeper's path
if (abs(self.cur_ball[Y] - self.cur_posture[id][Y]) > 2 * self.robot_size[id]):
# try to get ahead of the ball
self.set_target_position(id, self.cur_ball[X] - self.robot_size[id], self.cur_posture[id][Y], 1.4, 5.0,
0.4, False)
else:
# just give up and try not to make a suicidal goal
self.angle(id, math.pi / 2)
# if the ball is ahead of the goalkeeper
else:
desired_th = self.direction_angle(id, self.cur_ball[X], self.cur_ball[Y])
rad_diff = helper.trim_radian(desired_th - self.cur_posture[id][TH])
# if the robot direction is too away from the ball direction
if (rad_diff > math.pi / 3):
# give up kicking the ball and block the goalpost
self.set_target_position(id, x, y, 1.4, 5.0, 0.4, False)
else:
# try to kick the ball away from the goal
self.set_target_position(id, self.cur_ball[X], self.cur_ball[Y], 1.4, 3.0, 0.8, True)
# if the ball is not in the penalty area
else:
# if the ball is within alert range and y position is not too different
if (self.cur_ball[X] < -self.field[X] / 2 + 1.5 * self.penalty_area[X] and abs(
self.cur_ball[Y]) < 1.5 * self.penalty_area[Y] / 2 and abs(
self.cur_ball[Y] - self.cur_posture[id][Y]) < 0.2):
self.face_specific_position(id, self.cur_ball[X], self.cur_ball[Y])
# otherwise
else:
self.set_target_position(id, x, y, 1.4, 5.0, 0.4, True)
def angle(self, id, desired_th):
mult_ang = 0.4
d_th = desired_th - self.cur_posture[id][TH]
d_th = helper.trim_radian(d_th)
# the robot instead puts the direction rear if the angle difference is large
if (d_th > helper.d2r(95)):
d_th -= math.pi
sign = -1
elif (d_th < helper.d2r(-95)):
d_th += math.pi
sign = -1
self.set_wheel_velocity(id, -mult_ang * d_th, mult_ang * d_th, False)
def FLC(xRef=None, yRef=None, thRef=None, x=None, y=None, th=None, mode=None):
Ku = 12
if (mode == 'position'):
xError = xRef - x
yError = yRef - y
THRef = math.atan2(yError, xError)
thError = helper.trim_radian(THRef - th)
distError = math.sqrt(math.pow(xRef - x, 2) + math.pow(yRef - y, 2))
# Stop giving robot input if it's pretty close to the target
if (abs(distError) <= 0.05):
return 0, 0, thError, distError
if (mode == 'angle'):
distError = 0
thError = helper.trim_radian(thRef - th)
# If the angle is close to enough, stop the robot (or else it would drive slightly forward for some reason)
#if (abs(thError)< 0.05):
# return 0, 0, thError, distError
cmToM = pow(10, -3)
# Rule base for right wheel
ruleR[0] = strength(distError, NBd, thError, NBth)
outR[0] = -35 * cmToM
ruleR[1] = strength(distError, NBd, thError, NMth)
outR[1] = -25 * cmToM
ruleR[2] = strength(distError, NBd, thError, NSth)
outR[2] = -15 * cmToM
ruleR[3] = strength(distError, NBd, thError, ZOth)
outR[3] = 30 * cmToM
ruleR[4] = strength(distError, NBd, thError, PSth)
outR[4] = 15 * cmToM
ruleR[5] = strength(distError, NBd, thError, PMth)
outR[5] = 25 * cmToM
ruleR[6] = strength(distError, NBd, thError, PBth)
outR[6] = 35 * cmToM
ruleR[7] = strength(distError, NMd, thError, NBth)
outR[7] = -27 * cmToM
ruleR[8] = strength(distError, NMd, thError, NMth)
outR[8] = 8 * cmToM
ruleR[9] = strength(distError, NMd, thError, NSth)
outR[9] = 15 * cmToM
ruleR[10] = strength(distError, NMd, thError, ZOth)
outR[10] = 30 * cmToM
ruleR[11] = strength(distError, NMd, thError, PSth)
outR[11] = 40 * cmToM
ruleR[12] = strength(distError, NMd, thError, PMth)
outR[12] = 51 * cmToM
ruleR[13] = strength(distError, NMd, thError, PBth)
outR[13] = 63 * cmToM
ruleR[14] = strength(distError, NSd, thError, NBth)
outR[14] = -27 * cmToM
ruleR[15] = strength(distError, NSd, thError, NMth)
outR[15] = 8 * cmToM
ruleR[16] = strength(distError, NSd, thError, NSth)
outR[16] = 22 * cmToM
ruleR[17] = strength(distError, NSd, thError, ZOth)
outR[17] = 50 * cmToM
ruleR[18] = strength(distError, NSd, thError, PSth)
outR[18] = 44 * cmToM
ruleR[19] = strength(distError, NSd, thError, PMth)
outR[19] = 51 * cmToM
ruleR[20] = strength(distError, NSd, thError, PBth)
outR[20] = 63 * cmToM
ruleR[21] = strength(distError, ZOd, thError, NBth)
outR[21] = -3 * cmToM
ruleR[22] = strength(distError, ZOd, thError, NMth)
outR[22] = 18 * cmToM
ruleR[23] = strength(distError, ZOd, thError, NSth)
outR[23] = 35 * cmToM
ruleR[24] = strength(distError, ZOd, thError, ZOth)
outR[24] = 60 * cmToM
ruleR[25] = strength(distError, ZOd, thError, PSth)
outR[25] = 65 * cmToM
ruleR[26] = strength(distError, ZOd, thError, PMth)
outR[26] = 61 * cmToM
ruleR[27] = strength(distError, ZOd, thError, PBth)
outR[27] = 67 * cmToM
ruleR[28] = strength(distError, PSd, thError, NBth)
outR[28] = -3 * cmToM
ruleR[29] = strength(distError, PSd, thError, NMth)
outR[29] = 31 * cmToM
ruleR[30] = strength(distError, PSd, thError, NSth)
outR[30] = 57 * cmToM
ruleR[31] = strength(distError, PSd, thError, ZOth)
outR[31] = 90 * cmToM
ruleR[32] = strength(distError, PSd, thError, PSth)
outR[32] = 82 * cmToM
ruleR[33] = strength(distError, PSd, thError, PMth)
outR[33] = 68 * cmToM
ruleR[34] = strength(distError, PSd, thError, PBth)
outR[34] = 67 * cmToM
ruleR[35] = strength(distError, PMd, thError, NBth)
outR[35] = -3 * cmToM
ruleR[36] = strength(distError, PMd, thError, NMth)
outR[36] = 31 * cmToM
ruleR[37] = strength(distError, PMd, thError, NSth)
outR[37] = 67 * cmToM
ruleR[38] = strength(distError, PMd, thError, ZOth)
outR[38] = 100 * cmToM
ruleR[39] = strength(distError, PMd, thError, PSth)
outR[39] = 92 * cmToM
ruleR[40] = strength(distError, PMd, thError, PMth)
outR[40] = 68 * cmToM
ruleR[41] = strength(distError, PMd, thError, PBth)
outR[41] = 67 * cmToM
ruleR[42] = strength(distError, PBd, thError, NBth)
outR[42] = -3 * cmToM
ruleR[43] = strength(distError, PBd, thError, NMth)
outR[43] = 42 * cmToM
ruleR[44] = strength(distError, PBd, thError, NSth)
outR[44] = 67 * cmToM
ruleR[45] = strength(distError, PBd, thError, ZOth)
outR[45] = 100 * cmToM
ruleR[46] = strength(distError, PBd, thError, PSth)
outR[46] = 92 * cmToM
ruleR[47] = strength(distError, PBd, thError, PMth)
outR[47] = 77 * cmToM
ruleR[48] = strength(distError, PBd, thError, PBth)
outR[48] = 67 * cmToM
# Rule base for left wheel (equal to flipped rule base of right wheel for thError = 0)
ruleL[0] = strength(distError, NBd, thError, NBth)
outL[0] = 35 * cmToM
ruleL[1] = strength(distError, NBd, thError, NMth)
outL[1] = 25 * cmToM
ruleL[2] = strength(distError, NBd, thError, NSth)
outL[2] = 15 * cmToM
ruleL[3] = strength(distError, NBd, thError, ZOth)
outL[3] = 30 * cmToM
ruleL[4] = strength(distError, NBd, thError, PSth)
outL[4] = -15 * cmToM
ruleL[5] = strength(distError, NBd, thError, PMth)
outL[5] = -25 * cmToM
ruleL[6] = strength(distError, NBd, thError, PBth)
outL[6] = -35 * cmToM
ruleL[7] = strength(distError, NMd, thError, NBth)
outL[7] = 63 * cmToM
ruleL[8] = strength(distError, NMd, thError, NMth)
outL[8] = 51 * cmToM
ruleL[9] = strength(distError, NMd, thError, NSth)
outL[9] = 40 * cmToM
ruleL[10] = strength(distError, NMd, thError, ZOth)
outL[10] = 30 * cmToM
ruleL[11] = strength(distError, NMd, thError, PSth)
outL[11] = 15 * cmToM
ruleL[12] = strength(distError, NMd, thError, PMth)
outL[12] = 8 * cmToM
ruleL[13] = strength(distError, NMd, thError, PBth)
outL[13] = -27 * cmToM
ruleL[14] = strength(distError, NSd, thError, NBth)
outL[14] = 63 * cmToM
ruleL[15] = strength(distError, NSd, thError, NMth)
outL[15] = 51 * cmToM
ruleL[16] = strength(distError, NSd, thError, NSth)
outL[16] = 44 * cmToM
ruleL[17] = strength(distError, NSd, thError, ZOth)
outL[17] = 50 * cmToM
ruleL[18] = strength(distError, NSd, thError, PSth)
outL[18] = 22 * cmToM
ruleL[19] = strength(distError, NSd, thError, PMth)
outL[19] = 8 * cmToM
ruleL[20] = strength(distError, NSd, thError, PBth)
outL[20] = -27 * cmToM
ruleL[21] = strength(distError, ZOd, thError, NBth)
outL[21] = 67 * cmToM
ruleL[22] = strength(distError, ZOd, thError, NMth)
outL[22] = 61 * cmToM
ruleL[23] = strength(distError, ZOd, thError, NSth)
outL[23] = 65 * cmToM
ruleL[24] = strength(distError, ZOd, thError, ZOth)
outL[24] = 60 * cmToM
ruleL[25] = strength(distError, ZOd, thError, PSth)
outL[25] = 35 * cmToM
ruleL[26] = strength(distError, ZOd, thError, PMth)
outL[26] = 18 * cmToM
ruleL[27] = strength(distError, ZOd, thError, PBth)
outL[27] = -3 * cmToM
ruleL[28] = strength(distError, PSd, thError, NBth)
outL[28] = 67 * cmToM
ruleL[29] = strength(distError, PSd, thError, NMth)
outL[29] = 68 * cmToM
ruleL[30] = strength(distError, PSd, thError, NSth)
outL[30] = 82 * cmToM
ruleL[31] = strength(distError, PSd, thError, ZOth)
outL[31] = 90 * cmToM
ruleL[32] = strength(distError, PSd, thError, PSth)
outL[32] = 57 * cmToM
ruleL[33] = strength(distError, PSd, thError, PMth)
outL[33] = 31 * cmToM
ruleL[34] = strength(distError, PSd, thError, PBth)
outL[34] = -3 * cmToM
ruleL[35] = strength(distError, PMd, thError, NBth)
outL[35] = 67 * cmToM
ruleL[36] = strength(distError, PMd, thError, NMth)
outL[36] = 68 * cmToM
ruleL[37] = strength(distError, PMd, thError, NSth)
outL[37] = 92 * cmToM
ruleL[38] = strength(distError, PMd, thError, ZOth)
outL[38] = 100 * cmToM
ruleL[39] = strength(distError, PMd, thError, PSth)
outL[39] = 67 * cmToM
ruleL[40] = strength(distError, PMd, thError, PMth)
outL[40] = 31 * cmToM
ruleL[41] = strength(distError, PMd, thError, PBth)
outL[41] = -3 * cmToM
ruleL[42] = strength(distError, PBd, thError, NBth)
outL[42] = 67 * cmToM
ruleL[43] = strength(distError, PBd, thError, NMth)
outL[43] = 77 * cmToM
ruleL[44] = strength(distError, PBd, thError, NSth)
outL[44] = 92 * cmToM
ruleL[45] = strength(distError, PBd, thError, ZOth)
outL[45] = 100 * cmToM
ruleL[46] = strength(distError, PBd, thError, PSth)
outL[46] = 67 * cmToM
ruleL[47] = strength(distError, PBd, thError, PMth)
outL[47] = 42 * cmToM
ruleL[48] = strength(distError, PBd, thError, PBth)
outL[48] = -3 * cmToM
return Ku * defuzzy(ruleL, outL), Ku * defuzzy(ruleR,
outR), thError, distError
def is_facing_target(self, id, x, y):
desiredTh = self.direction_angle(id, x, y)
thError = helper.trim_radian(desiredTh - self.cur_posture[id][TH])
return (abs(thError) < 0.2)
