Merge pull request #140 from project-neon/feat/rcx2023

Feat/rcx2023

algorithms/__init__.py

@@ -12,4 +12,6 @@
 
 from algorithms import univector_field
 
+from algorithms.univector_field import UnivectorField
+
 from algorithms.RRT import rrt

comm/rl_comm.py

@@ -40,7 +40,7 @@ def send(self, robot_commands = []):
         message = "<"
         robot_commands = sorted(robot_commands, key = lambda i: i['robot_id'])
         for rb in robot_commands:
-            message += f"{rb['robot_id']},{round(rb['wheel_left'], 2)},{round(rb['wheel_right'], 2)},"
+            message += f"{rb['robot_id']},{round(rb['wheel_left'], 4)},{round(rb['wheel_right'], 4)},"
 
         message = message[:-1] + '>'
 

config_real_life.json

@@ -16,7 +16,7 @@
     "match" : {
         "team_side": "left",
         "team_color": "yellow",
-        "coach_name": "RSM_2023",
+        "coach_name": "RCX_2023",
         "category": "3v3",
         "robot_ids": [
             1,

controller/PID_control.py

@@ -59,13 +59,13 @@ class PID_control(object):
             # Control params
             'K_RHO': 500, # Linear speed gain
             # PID of angular speed
-            'KP': -1000, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO
+            'KP': -3.5, # -700, # Proportional gain of w (angular speed), respecting the stability condition: K_RHO > 0 and KP > K_RHO
             'KD': 0, # -180, # Derivative gain of w
-            'KI': 0, # Integral gain of w
+            'KI': 0  , # Integral gain of w
             # Max speeds for the robot
-            'V_MAX': 150, # linear speed
-            'W_MAX': -1, # angular speed rad/s
-            'V_MIN': 20,
+            'V_MAX': 0.3, # linear speed
+            'W_MAX': -0.5, # angular speed rad/s
+            'V_MIN': 0.1,
 
             'TWO_SIDES': True
         }
@@ -146,7 +146,7 @@ def _update(self):
 
         self.alpha_old = alpha
 
-        return v, w
+        return -v, w
 
     def update(self):
         v, w = self._update()

controller/uni_controller.py

@@ -39,10 +39,10 @@ class UniController(object):
             'K_P': 10
         },
         'real_life': {
-            'V_M': 10000,
-            'R_M': 3 * 10000, # 20 * V_M
-            'K_W': 270, # 313,
-            'K_P': 100
+            'V_M': 0.5,
+            'R_M': 0.44, # 20 * V_M
+            'K_W': 3.5, # 313,
+            'K_P': 1
         }
     }
 
@@ -133,5 +133,7 @@ def update(self):
 
         if self.environment == 'simulation':
             return tuple(np.dot(250, speed_to_power(v, w, self.L, self.R)))
-
-        return v, -w
+
+
+        # w = w if abs(w) < 4 else 4 * w / abs(w)
+        return -v, -w

entities/coach/__init__.py

@@ -12,13 +12,16 @@
 
 from entities.coach.rsm2023 import Coach as RSM_2023
 
+from entities.coach.rcx2023 import Coach as RCX_2023
+
 _coach_list = [
     # Tournament coaches
     GuideCoach,
     RSMCoach,
     IRON_2022,
     IRON_2023,
     RSM_2023,
+    RCX_2023,
     TestCoach
 ]
 

entities/coach/rcx2023.py

@@ -0,0 +1,70 @@
+from commons.math import distance_between_points
+from entities.coach.coach import BaseCoach
+import strategy
+
+
+class Coach(BaseCoach):
+    NAME = "RCX_2023"
+
+    def __init__(self, match):
+        super().__init__(match)
+
+        self.SS_strategy = strategy.rcx2023.ShadowAttacker(self.match)
+        self.ST_strategy = strategy.rcx2023.MainStriker(self.match)
+        self.GK_strategy = strategy.rcx2023.Goalkeeper(self.match)
+        self.GK_id = 1  # Goalkeeper fixed ID
+
+        # self.unstucks = {r.robot_id: strategy.rsm2023.Unstuck(self.match) for r in self.match.robots if r.robot_id != self.GK_id}
+
+    def decide(self):
+        GK = [i for i, r in enumerate(self.match.robots) if r.robot_id is self.GK_id][0]
+        strikers = [r for i, r in enumerate(self.match.robots) if r.robot_id is not self.GK_id]
+        ST, SS = self.choose_main_striker(*strikers)
+
+        st_strat, ss_start = self.handle_stuck(ST, SS)
+
+        if self.match.robots[GK].strategy is None:
+            self.match.robots[GK].strategy = self.GK_strategy
+            self.match.robots[GK].start()
+        else:
+            if self.match.robots[GK].strategy.name != self.GK_strategy:
+                self.match.robots[GK].strategy = self.GK_strategy
+                self.match.robots[GK].start()
+
+        ST.strategy = st_strat
+        ST.start()
+
+        SS.strategy = ss_start
+        SS.start()
+
+    def choose_main_striker(self, r1, r2):
+        b = self.match.ball
+
+        a1 = distance_between_points((b.x, b.y), (r1.x, r1.y))
+        a2 = distance_between_points((b.x, b.y), (r2.x, r2.y))
+
+        b1, b2 = b.x - r1.x, b.x - r2.x
+
+        if b1 * b2 > 0:
+            if a1 < a2:
+                return r1, r2
+            return r2, r1
+        if b1 > 0:
+            return r1, r2
+        return r2, r1
+
+    def handle_stuck(self, ST, SS):
+        game_runing = not (self.match.game_status == 'STOP' or self.match.game_status == None)
+        stuck_st = ST.is_stuck() and game_runing
+        stuck_ss = SS.is_stuck() and game_runing
+
+        # if stuck_st and stuck_ss:
+        #     return self.unstucks[ST.robot_id], self.unstucks[SS.robot_id]
+        #
+        # if stuck_st and not stuck_ss:
+        #     return self.unstucks[ST.robot_id], self.ST_strategy
+        #
+        # if not stuck_st and stuck_ss:
+        #     return self.ST_strategy, self.unstucks[SS.robot_id]
+
+        return self.ST_strategy, self.SS_strategy

entities/coach/test_coach.py

@@ -8,8 +8,8 @@ def __init__(self, match):
         super().__init__(match) # chamada do metodo da classe mae
 
         # vamos usar strategies de teste por enquanto, essa deixa o robo parado
-        self._1 = strategy.tests.Idle(self.match)
-        self._2 = strategy.tests.Foward(self.match)
+        self._1 = strategy.tests.PIDTuner(self.match)
+        self._2 = strategy.tests.Idle(self.match)
         self._3 = strategy.tests.Idle(self.match)
 
     def decide(self):

strategy/__init__.py

@@ -5,6 +5,7 @@
 from strategy import cbfr2022
 from strategy import iron2023
 from strategy import rsm2023
+from strategy import rcx2023
 from strategy.BaseStrategy import Strategy
 
 # debug tools