def get_goals(self):
"""
Gibt einem die Position der beiden Tore als Liste zurück
:return: list of tupel (u,v koordinaten)
"""
distance1, angel1 = self._get_goal_middel_poolar(0)
distance2, angel2 = self._get_goal_middel_poolar(1)
return [convert_polar2uv(distance1, angel1), convert_polar2uv(distance2, angel2)]
def _calculate_move(self, delta_distance, angel):
"""
Berechnet die u/v verschiebung die nötig ist um die delta_distance
an der referens von angel zu verschieben
"""
return convert_polar2uv(delta_distance, angel)
def get_uv(self):
return convert_polar2uv(self.distance, self.angel)
def get_opp_goal(self):
"""
Gibt Position des gegnerischen Tores zurück
"""
return convert_polar2uv(*self._get_goal_middel_poolar((self.own_goal + 1) % 2))
def get_own_goal(self):
"""
Gibt Position des eigenen Tores zurück
"""
return convert_polar2uv(*self._get_goal_middel_poolar(self.own_goal))
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)