def _correction_rotation(self, t, post_u, post_v, image_u, image_v):
"""
Korrigiert die Drehung der beiden Tore nachdem die Tormittelpunkte korrigiert wurden.
"""
# dreh winkel herausfinden
post_distance, post_angel = convert_uv2polar(post_u, post_v)
image_distance, image_angel = convert_uv2polar(image_u, image_v)
# print image_angel, post_angel
correction_angle = image_angel - post_angel
def reset_to_own(self):
# initilize with position in middle of our side
config = get_config()['field']
self.length = config['length']
goal_width = config['goal-width']
self.goal_posts.append(GoalPost(*convert_uv2polar(-self.length / 4, -goal_width / 2)))
self.goal_posts.append(GoalPost(*convert_uv2polar(-self.length / 4, goal_width / 2)))
self.goal_posts.append(GoalPost(*convert_uv2polar(self.length / 4 * 3, goal_width / 2)))
self.goal_posts.append(GoalPost(*convert_uv2polar(self.length / 4 * 3, -goal_width / 2)))
# am anfang sollte das Tor 0 unseres sein
self.own_goal = config['own-goal']
self.object_list['middle_point'] = LocalObject(*convert_uv2polar(self.length / 4, 0))
def _match_one(self, goal_info):
"""
Findet bei nur einem sichtbaren Torpfosten heraus welche von den
gemerkten Positionen die zugehörige ist und korrigiert diese dann
anhand des gesehenen Torpfostens. Auch alle anderen Torpfosten
werden um die gleichen Werte verschoben, weil unsere Odometrie
wohl falsche Werte geliefert hat und dies betrifft dann auch die
anderen Pfosten.
"""
distance, angel = convert_uv2polar(goal_info.u, goal_info.v)
if distance < 1:
# wenn es weniger als 1mm ist wirds unten doof
distance = 1
min_delta = 99999999999999
min_goal_post = None
# hier wird der nähste Pfosten gesucht
for goal_post in self.goal_posts:
# abs weil floatmagie
delta = sqrt(abs(
distance ** 2 + goal_post.distance ** 2 -
2 * distance * goal_post.distance * cos(angel - goal_post.angel)))
if delta < min_delta:
min_delta = delta
min_goal_post = goal_post
delta_distance = distance - min_goal_post.distance
delta_angel = angel - min_goal_post.angel
# jetzt werden alle Pfosten neu lokalisiert
if delta_distance > 1000: # todo wert evaluieren und config
# wir rotieren nur, wenn wir weit genug weg sind
self.rotate(self._wrap_angel(delta_angel))
self.move(*self._calculate_move(delta_distance, min_goal_post.angel))
def perform(self, connector, reevaluate=False):
# search complete goal
connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=0, own_goal_priority=0,
align_priority=1510)
connector.blackboard_capsule().cancel_ball_tracking()
connector.walking_capsule().stop_walking()
self.do_not_reevaluate()
# waits for result from head
if not connector.blackboard_capsule().get_complete_goal_found() and \
not connector.blackboard_capsule().get_cant_find_complete_goal():
return self.push(Wait, 0.1)
# if found
if connector.blackboard_capsule().get_complete_goal_found():
connector.blackboard_capsule().unset_complete_goal_found()
goal_center = connector.blackboard_capsule().get_complete_goal_center()
goal_distance = math.sqrt(goal_center[0] ** 2 + goal_center[1] ** 2)
goal_polar = convert_uv2polar(goal_center[0], goal_center[1])
goal_angle = math.degrees(goal_polar[1])
# test if own goal
right_goal = self.test_if_right_goal(connector, goal_distance)
goal_v = goal_center[1]
# compute angle to move
if not right_goal:
say("Do not shot on own goal")
sign = self.sign(goal_angle)
goal_angle += 180 * sign
# ausgleich, wenns über 180 rüber geht, weil unsere polarwerte so funktionieren
# math if it is more than 180, because our polarvalues work this way
if goal_angle < -180:
goal_angle = 180 - (abs(goal_angle) - 180)
elif goal_angle > 180:
goal_angle = -180 + (abs(goal_angle) - 180)
if goal_v > 0:
goal_v -= 10000
else:
goal_v += 10000
if False:
# if goodangle
if abs(goal_angle) < 30:
# shoot
connector.blackboard_capsule().set_finished_align()
connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=1000,
own_goal_priority=0, align_priority=0)
say("kick")
return self.interrupt()
# else (useful comment...)
else:
say("start")
# feste zahl drehen
# turn fixed value
factor = 0.2
# chose direction
if self.sign(goal_angle) == 1:
return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.MEDIUM_ANGULAR_LEFT,
WalkingCapsule.SLOW_SIDEWARDS_RIGHT,
abs(goal_angle) * factor]])
else:
return self.push(PlainWalkAction, [[WalkingCapsule.ZERO, WalkingCapsule.MEDIUM_ANGULAR_RIGHT,
WalkingCapsule.SLOW_SIDEWARDS_LEFT,
abs(goal_angle) * factor]])
# warten abbuf ende der drehung nicht nötig,
# da wir dann einfach wieder oben anfangen und neu nach dem tor suchen
if False: # should work
if goal_angle < -30:
say("Kick Ball right")
return self.push(KickBall, init_data="SRK")
elif goal_angle > 30:
say("Kick Ball left")
return self.push(KickBall, init_data="SLK")
else:
if connector.raw_vision_capsule().get_ball_info("v") <= 0:
say("Kick Strong")
return self.push(KickBall, init_data="RP")
else:
say("Kick Strong")
return self.push(KickBall, init_data="LP")
if True:
if (goal_distance > 2000 and abs(goal_angle > 30)) or (goal_distance <= 2000 and abs(goal_v) > 1000):
if goal_angle < 0:
say("Kick Ball right")
return self.push(KickBall, init_data="SRK")
elif goal_angle >= 0:
say("Kick Ball left")
return self.push(KickBall, init_data="SLK")
else:
if connector.raw_vision_capsule().get_ball_info("v") <= 0:
say("Kick Strong")
return self.push(KickBall, init_data="RP")
else:
say("Kick Strong")
return self.push(KickBall, init_data="LP")
else:
connector.blackboard_capsule().unset_cant_find_complete_goal()
# shoot, to do at least anything
connector.blackboard_capsule().set_finished_align()
say("Cant find goal")
connector.blackboard_capsule().set_priorities(enemy_goal_priority=0, ball_priority=1000,
own_goal_priority=0, align_priority=0)
if connector.raw_vision_capsule().get_ball_info("v") <= 0:
say("Kick Strong")
return self.push(KickBall, init_data="RP")
else:
say("Kick Strong")
return self.push(KickBall, init_data="LP")
def set_position(self, u, v):
self.distance, self.angel = convert_uv2polar(u, v)
def move(self, u, v):
"""
Verschiebt die Koordinaten um u, v im Khartesischen Koordinatensystem
"""
u1, v1 = self.get_uv()
self.distance, self.angel = convert_uv2polar(u + u1, v + v1)
def _match_two(self, goal_info1, goal_info2):
"""
Funktioniert wie match_one bloß, dass der Mittelpunkt der Tore
genommen wird um die Verschiebung zur Odometrie zu bestimmen.
"""
distance1, angel1 = convert_uv2polar(goal_info1.u, goal_info1.v)
distance2, angel2 = convert_uv2polar(goal_info2.u, goal_info2.v)
if distance1 < 1:
# wenn es weniger als 1mm ist wirds unten doof
distance1 = 1
if distance2 < 1:
# wenn es weniger als 1mm ist wirds unten doof
distance2 = 1
# Mittelpunkt des gesehenen Tors
avg_distance = (distance1 + distance2) / 2
avg_angel = (angel1 + angel2) / 2
# Mittelpunkte der gespeicherten Tore
avg_goal1_distance, avg_goal1_angel = self._get_goal_middel_poolar(0)
avg_goal2_distance, avg_goal2_angel = self._get_goal_middel_poolar(1)
# Differenz zwischen gespeicherten Toren und dem gesehen
# abs weil float magie, machmal ist der inner ausdruck endgegend
# der Mathematik negativ
delta1 = sqrt(abs(
avg_distance ** 2 + avg_goal1_distance ** 2 -
2 * avg_distance * avg_goal1_distance * cos(avg_angel - avg_goal1_angel)))
delta2 = sqrt(abs(
avg_distance ** 2 + avg_goal2_distance ** 2 -
2 * avg_distance * avg_goal2_distance * cos(avg_angel - avg_goal2_angel)))
# entscheidung welches Tor das richtige ist
if delta1 < delta2:
delta_distance = avg_distance - avg_goal1_distance
delta_angel = avg_angel - avg_goal1_angel
goal = 0
else:
delta_distance = avg_distance - avg_goal2_distance
delta_angel = avg_angel - avg_goal2_angel
goal = 1
# korrektur der gespeicherten Daten
self.rotate(self._wrap_angel(delta_angel))
self.move(*self._calculate_move(delta_distance, self._get_goal_middel_poolar(goal)[1]))
# drehung der in sich Tore korrigieren
# rausfinden welches das gemachte tor ist
if delta1 < delta2:
post1 = self.goal_posts[0]
post2 = self.goal_posts[1]
avg_distance, avg_angel = self._get_goal_middel_poolar(0)
else:
post1 = self.goal_posts[2]
post2 = self.goal_posts[3]
avg_distance, avg_angel = self._get_goal_middel_poolar(1)
turnpoint_u, turnpoint_v = convert_polar2uv(avg_distance, avg_angel)
post_u, post_v = post1.get_uv()
# schaun welche goalinfo die richtige von dem torpfosten ist
delta1_u = abs(post_u - goal_info1.u)
delta1_v = abs(post_v - goal_info1.v)
delta2_u = abs(post_u - goal_info2.u)
delta2_v = abs(post_v - goal_info2.v)
# drehpunkt in tor mitte verschieben
# drehpunkt u und v
# alle pfosten transformieren
self.move(-turnpoint_u, -turnpoint_v, False)
post_u, post_v = post1.get_uv()
if delta1_u + delta1_v < delta2_u + delta2_v:
self._correction_rotation(1, post_u, post_v, goal_info1.u - turnpoint_u, goal_info1.v - turnpoint_v)
else:
self._correction_rotation(1, post_u, post_v, goal_info2.u - turnpoint_u, goal_info2.v - turnpoint_v)
# Transformation rückgängig machen
self.move(turnpoint_u, turnpoint_v, False)
