From d178436afb125feded4e64af51db813847a511bf Mon Sep 17 00:00:00 2001
From: aelhabashy <96305832+aelhabashy@users.noreply.github.com>
Date: Thu, 7 Nov 2024 14:17:48 +0100
Subject: [PATCH] reduced rate, fixed ball placement

---
 roboteam_ai/src/RL/env.py              | 27 ++++++++++++++++++++++----
 roboteam_ai/src/RL/src/getState.py     | 14 +++++++++++--
 roboteam_ai/src/RL/src/teleportBall.py |  4 ++--
 3 files changed, 37 insertions(+), 8 deletions(-)

diff --git a/roboteam_ai/src/RL/env.py b/roboteam_ai/src/RL/env.py
index 16a8bbc53..e0ccb7785 100644
--- a/roboteam_ai/src/RL/env.py
+++ b/roboteam_ai/src/RL/env.py
@@ -67,13 +67,28 @@ def __init__(self):
 
     def check_ball_placement(self):
         """
-        Function to teleport the ball to the designated position for ball placement if necessary
+        Function to teleport the ball to the designated position for ball placement if necessary.
         """
+        # Get the current referee state
+        referee_state, referee_info = get_referee_state()  # Assuming get_referee_state() is in scope
+
+        # Extract the command from the referee state
+        self.ref_command = referee_info['command']
+        print("ref command", self.ref_command)
+ 
 
         # If ref gives command BALL_PLACEMENT_US OR BALL_PLACEMENT_THEM
-        if (self.ref_command == "BALL_PLACEMENT_US") or ("BALL_PLACEMENT_THEM"):
-            teleport_ball(self.x, self.y)   # Teleport the ball to the designated location
-            
+        if (self.ref_command == 16 or self.ref_command == 17):
+            referee_state, referee_data = get_referee_state()
+
+            if referee_data["designated_position"]is not None:
+                self.x, self.y = referee_data["designated_position"]["x"]/1000, referee_data["designated_position"]["y"]/1000
+
+                # Teleport the ball to the designated location
+                teleport_ball(self.x, self.y)
+            else:
+                print("No designated position provided in referee state.")
+
 
     def get_referee_state(self):
         """
@@ -171,8 +186,12 @@ def step(self, action):
         observation_space,_ = self.get_observation()
 
         done = self.is_terminated()  # If task is completed (a goal was scored)
+        if(done):
+            reset()
         truncated = self.is_truncated()  # Determine if the episode was truncated, too much time or a yellow card
 
+        time.sleep(0.25)  # DELAY FOR STEPS (ADJUST LATER)
+
         return observation_space, reward, done, truncated, {}
 
 
diff --git a/roboteam_ai/src/RL/src/getState.py b/roboteam_ai/src/RL/src/getState.py
index 731daaebf..99881306e 100644
--- a/roboteam_ai/src/RL/src/getState.py
+++ b/roboteam_ai/src/RL/src/getState.py
@@ -158,12 +158,22 @@ def get_referee_state():
             "ball_placement_failures": blue_team_info.ball_placement_failures
         }
 
+        # Return both the raw referee_state object and a dictionary of extracted details
+        if referee_state.HasField('designated_position'):
+            designated_position = {
+                "x": referee_state.designated_position.x,
+                "y": referee_state.designated_position.y
+            }
+        else:
+            designated_position = None
+
         # Return both the raw referee_state object and a dictionary of extracted details
         return referee_state, {
             "yellow_team": yellow_team_data,
             "blue_team": blue_team_data,
             "stage": referee_state.stage,
-            "command": referee_state.command
+            "command": referee_state.command,
+            "designated_position": designated_position
         }
 
     except DecodeError:
@@ -204,4 +214,4 @@ def get_referee_state():
     print(f"Score: {blue_team['score']}, Red Cards: {blue_team['red_cards']}, Yellow Cards: {blue_team['yellow_cards']}")
     print(f"Fouls: {blue_team['fouls']}, Ball Placement Failures: {blue_team['ball_placement_failures']}")
 
-    get_ball_state()
+    print(get_referee_state())
diff --git a/roboteam_ai/src/RL/src/teleportBall.py b/roboteam_ai/src/RL/src/teleportBall.py
index 01d9b59a7..563dd8577 100644
--- a/roboteam_ai/src/RL/src/teleportBall.py
+++ b/roboteam_ai/src/RL/src/teleportBall.py
@@ -13,7 +13,7 @@
 
 # Make sure to go back to the main roboteam directory
 current_dir = os.path.dirname(os.path.abspath(__file__))
-roboteam_path = os.path.abspath(os.path.join(current_dir, "..", "..", ".."))
+roboteam_path = os.path.abspath(os.path.join(current_dir, "..","..", "..", ".."))
 
 # Add to sys.path
 sys.path.append(roboteam_path)
@@ -50,5 +50,5 @@ def teleport_ball(x, y, z=0.0):
 
 if __name__ == "__main__":
 
-    teleport_ball(400, 4000)
+    teleport_ball(5.000, 4.300, 0.0)
     print("Script execution completed")
\ No newline at end of file