コード例 #1
0
 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)]
コード例 #2
0
    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)
コード例 #3
0
 def get_uv(self):
     return convert_polar2uv(self.distance, self.angel)
コード例 #4
0
 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))
コード例 #5
0
 def get_own_goal(self):
     """
     Gibt Position des eigenen Tores zurück
     """
     return convert_polar2uv(*self._get_goal_middel_poolar(self.own_goal))
コード例 #6
0
    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)