From 26dd1e66c59e99e9ce6308bdeb7e854f63fdb2ff Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 12:38:57 +0000 Subject: [PATCH 1/8] add kick persistance in transmission --- utama_core/config/settings.py | 1 + .../controllers/real/real_robot_controller.py | 23 +++++++++++++++++-- 2 files changed, 22 insertions(+), 2 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index d07bef9d..0b81b83c 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -31,6 +31,7 @@ BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" TIMEOUT = 0.1 +KICK_PERSIST_TIMESTEPS = 3 # number of timesteps to keep kick HIGH after command MAX_GAME_HISTORY = 20 # number of previous game states to keep in Game diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 023a96d6..a9eec616 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -7,7 +7,13 @@ from serial import EIGHTBITS, PARITY_EVEN, STOPBITS_TWO, Serial from utama_core.config.robot_params import REAL_PARAMS -from utama_core.config.settings import BAUD_RATE, PORT, TIMEOUT, TIMESTEP +from utama_core.config.settings import ( + BAUD_RATE, + KICK_PERSIST_TIMESTEPS, + PORT, + TIMEOUT, + TIMESTEP, +) from utama_core.entities.data.command import RobotCommand, RobotResponse from utama_core.skills.src.utils.move_utils import empty_command from utama_core.team_controller.src.controllers.common.robot_controller_abstract import ( @@ -39,6 +45,9 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): logger.debug(f"Serial port: {PORT} opened with baudrate: {BAUD_RATE} and timeout {TIMEOUT}") self._assigned_mapping = {} # mapping of robot_id to index in the out_packet + # track last kick time for each robot to transmit kick as HIGH for n timesteps after command + self._kick_tracker = {} + def get_robots_responses(self) -> Optional[List[RobotResponse]]: ### TODO: Not implemented yet return None @@ -58,6 +67,13 @@ def send_robot_commands(self) -> None: # print(data_in) # TODO: add receiving feedback from the robots + for robot_id in list(self._kick_tracker.keys()): + if self._kick_tracker[robot_id] > 1: + self._kick_tracker[robot_id] -= 1 + else: + # reset kick command to 0 in the out_packet + del self._kick_tracker[robot_id] + self._out_packet = self._empty_command() # flush the out_packet self._assigned_mapping = {} # reset assigned mapping @@ -98,6 +114,9 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: command_buffer = self._generate_command_buffer(robot_id, c_command) self._out_packet[start_idx + 1 : start_idx + self._rbt_cmd_size + 1] = command_buffer + if command.kick and robot_id not in self._kick_tracker: + self._kick_tracker[robot_id] = KICK_PERSIST_TIMESTEPS + # def _populate_robots_info(self, data_in: bytes) -> None: # """ # Populates the robots_info list with the data received from the robots. @@ -140,7 +159,7 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by ) kicker_byte = 0 - if c_command.kick: + if c_command.kick or robot_id in self._kick_tracker: kicker_byte |= 0xF0 # upper kicker full power if c_command.chip: kicker_byte |= 0x0F From 78a8615212087939c4c4ace3b4b534f5f195a1c1 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 12:46:31 +0000 Subject: [PATCH 2/8] update values and add chip tracker --- utama_core/config/settings.py | 2 +- .../controllers/real/real_robot_controller.py | 17 ++++++++++++++++- 2 files changed, 17 insertions(+), 2 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 0b81b83c..f477c75e 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -31,7 +31,7 @@ BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" TIMEOUT = 0.1 -KICK_PERSIST_TIMESTEPS = 3 # number of timesteps to keep kick HIGH after command +KICK_PERSIST_TIMESTEPS = 600 # number of timesteps to keep kick HIGH after command (cooldown of 10s at 60Hz) MAX_GAME_HISTORY = 20 # number of previous game states to keep in Game diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index a9eec616..e4fda402 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -47,6 +47,7 @@ def __init__(self, is_team_yellow: bool, n_friendly: int): # track last kick time for each robot to transmit kick as HIGH for n timesteps after command self._kick_tracker = {} + self._chip_tracker = {} def get_robots_responses(self) -> Optional[List[RobotResponse]]: ### TODO: Not implemented yet @@ -67,6 +68,11 @@ def send_robot_commands(self) -> None: # print(data_in) # TODO: add receiving feedback from the robots + ### update kick and chip trackers. We persist the kick/chip command for KICK_PERSIST_TIMESTEPS + ### this feature is to combat packet loss and to ensure the robot does not kick within its cooldown period + ### Embedded only registers rising edge of kick/chip command + # TODO: this logic has to be reconsidered. Solution for qualification now. + for robot_id in list(self._kick_tracker.keys()): if self._kick_tracker[robot_id] > 1: self._kick_tracker[robot_id] -= 1 @@ -74,6 +80,13 @@ def send_robot_commands(self) -> None: # reset kick command to 0 in the out_packet del self._kick_tracker[robot_id] + for robot_id in list(self._chip_tracker.keys()): + if self._chip_tracker[robot_id] > 1: + self._chip_tracker[robot_id] -= 1 + else: + # reset chip command to 0 in the out_packet + del self._chip_tracker[robot_id] + self._out_packet = self._empty_command() # flush the out_packet self._assigned_mapping = {} # reset assigned mapping @@ -116,6 +129,8 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: if command.kick and robot_id not in self._kick_tracker: self._kick_tracker[robot_id] = KICK_PERSIST_TIMESTEPS + if command.chip and robot_id not in self._chip_tracker: + self._chip_tracker[robot_id] = KICK_PERSIST_TIMESTEPS # def _populate_robots_info(self, data_in: bytes) -> None: # """ @@ -147,7 +162,7 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by ) dribbler_speed = 0 - if c_command.dribble: + if c_command.dribble or robot_id in self._chip_tracker: dribbler_speed = 0xC000 # set bits 15:14 to 11 dribbler_speed |= 4095 & 0x3FFF # set bits 13:0 to 4095 From 99e3f69f6770cc6da7e5fecae94cda07d4e7e127 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 12:49:59 +0000 Subject: [PATCH 3/8] accidentally set dribbler high for chip instead of chip itself --- .../src/controllers/real/real_robot_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index e4fda402..3a0d9ebe 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -162,7 +162,7 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by ) dribbler_speed = 0 - if c_command.dribble or robot_id in self._chip_tracker: + if c_command.dribble: dribbler_speed = 0xC000 # set bits 15:14 to 11 dribbler_speed |= 4095 & 0x3FFF # set bits 13:0 to 4095 @@ -176,7 +176,7 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by kicker_byte = 0 if c_command.kick or robot_id in self._kick_tracker: kicker_byte |= 0xF0 # upper kicker full power - if c_command.chip: + if c_command.chip or robot_id in self._chip_tracker: kicker_byte |= 0x0F packet.append(kicker_byte) # Kicker controls # Frame end From 92f4028417e4721d7230b751698ad3d62c149a45 Mon Sep 17 00:00:00 2001 From: Joel <42647510+energy-in-joles@users.noreply.github.com> Date: Fri, 23 Jan 2026 12:51:10 +0000 Subject: [PATCH 4/8] Update utama_core/config/settings.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- utama_core/config/settings.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index f477c75e..a12b25ee 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -31,7 +31,9 @@ BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" TIMEOUT = 0.1 -KICK_PERSIST_TIMESTEPS = 600 # number of timesteps to keep kick HIGH after command (cooldown of 10s at 60Hz) +KICK_CHIP_PERSIST_TIMESTEPS = 600 # number of timesteps to keep kick/chip HIGH after command (cooldown of 10s at 60Hz) +# Backwards-compatible alias; prefer KICK_CHIP_PERSIST_TIMESTEPS in new code. +KICK_PERSIST_TIMESTEPS = KICK_CHIP_PERSIST_TIMESTEPS MAX_GAME_HISTORY = 20 # number of previous game states to keep in Game From d4bbab3bde9abd1582acc7dfa32757ac87257914 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 12:52:45 +0000 Subject: [PATCH 5/8] make kick persistance clearer --- utama_core/config/settings.py | 3 ++- .../src/controllers/real/real_robot_controller.py | 1 - 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index f477c75e..1df649be 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -31,7 +31,8 @@ BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" TIMEOUT = 0.1 -KICK_PERSIST_TIMESTEPS = 600 # number of timesteps to keep kick HIGH after command (cooldown of 10s at 60Hz) +KICK_PERSIST_TIME = 10 # in seconds to keep kick HIGH after command +KICK_PERSIST_TIMESTEPS = KICK_PERSIST_TIME * CONTROL_FREQUENCY MAX_GAME_HISTORY = 20 # number of previous game states to keep in Game diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 3a0d9ebe..68957bf5 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -189,7 +189,6 @@ def _convert_float16_command(self, robot_id, command: RobotCommand) -> RobotComm Also converts angular velocity to degrees per second. """ - angular_vel = self._sanitise_float(command.angular_vel) local_forward_vel = self._sanitise_float(command.local_forward_vel) local_left_vel = self._sanitise_float(command.local_left_vel) From d5279b3277629ff70c875d5b76d1a12581a741e7 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 13:16:38 +0000 Subject: [PATCH 6/8] prevent kick and chip at the same time --- .../src/controllers/real/real_robot_controller.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index c619a330..2c725307 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -179,10 +179,12 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by kicker_byte = 0 # check kick and chip command but also check the kicker tracker to see if we need to persist the command + # elif needed here to ensure the persistance command doesnt cause us to kick and chip at the same time if c_command.kick or (robot_id in self._kicker_tracker and self._kicker_tracker[robot_id].is_kick): kicker_byte |= 0xF0 # upper kicker full power - if c_command.chip or (robot_id in self._kicker_tracker and not self._kicker_tracker[robot_id].is_kick): + elif c_command.chip or (robot_id in self._kicker_tracker and not self._kicker_tracker[robot_id].is_kick): kicker_byte |= 0x0F + packet.append(kicker_byte) # Kicker controls # Frame end # packet_str = " ".join(f"{byte:08b}" for byte in packet) From 88cd6dbfb2ceadd58f694bf2cd7333671c2a0a52 Mon Sep 17 00:00:00 2001 From: Joel Date: Fri, 23 Jan 2026 23:29:42 +0000 Subject: [PATCH 7/8] separate cooldown from persist --- utama_core/config/settings.py | 5 +- .../controllers/real/real_robot_controller.py | 46 +++++++++++++++---- 2 files changed, 39 insertions(+), 12 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index fd438b0d..e2a2bce3 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -31,8 +31,9 @@ BAUD_RATE = 115200 PORT = "/dev/ttyUSB0" TIMEOUT = 0.1 -KICKER_PERSIST_TIME = 10 # in seconds to keep kick HIGH after command -KICKER_PERSIST_TIMESTEPS = KICKER_PERSIST_TIME * CONTROL_FREQUENCY +KICKER_COOLDOWN_TIME = 10 # in seconds to prevent kicker from being actuated too frequently +KICKER_COOLDOWN_TIMESTEPS = int(KICKER_COOLDOWN_TIME * CONTROL_FREQUENCY) # in timesteps +KICKER_PERSIST_TIMESTEPS = 10 # in timesteps to persist the kick command MAX_GAME_HISTORY = 20 # number of previous game states to keep in Game diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 2c725307..0ce77137 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -10,6 +10,7 @@ from utama_core.config.robot_params import REAL_PARAMS from utama_core.config.settings import ( BAUD_RATE, + KICKER_COOLDOWN_TIMESTEPS, KICKER_PERSIST_TIMESTEPS, PORT, TIMEOUT, @@ -30,7 +31,8 @@ @dataclasses.dataclass class KickTrackerEntry: - remaining_timesteps: int + remaining_persist: int + remaining_cooldown: int is_kick: bool # True for kick, False for chip @@ -80,8 +82,11 @@ def send_robot_commands(self) -> None: # TODO: this logic has to be reconsidered. Solution for qualification now. for robot_id in list(self._kicker_tracker.keys()): - if self._kicker_tracker[robot_id].remaining_timesteps > 1: - self._kicker_tracker[robot_id].remaining_timesteps -= 1 + if self._kicker_tracker[robot_id].remaining_cooldown > 1: + self._kicker_tracker[robot_id].remaining_cooldown -= 1 + + if self._kicker_tracker[robot_id].remaining_persist > 0: + self._kicker_tracker[robot_id].remaining_persist -= 1 else: # reset kick command to 0 in the out_packet del self._kicker_tracker[robot_id] @@ -128,12 +133,16 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: if command.kick and robot_id not in self._kicker_tracker: self._kicker_tracker[robot_id] = KickTrackerEntry( - remaining_timesteps=KICKER_PERSIST_TIMESTEPS, is_kick=True + remaining_persist=KICKER_PERSIST_TIMESTEPS, + remaining_cooldown=KICKER_COOLDOWN_TIMESTEPS, + is_kick=True, ) # if for some reason we are kicking and chipping at the same time, we prioritise kick if command.chip and robot_id not in self._kicker_tracker: self._kicker_tracker[robot_id] = KickTrackerEntry( - remaining_timesteps=KICKER_PERSIST_TIMESTEPS, is_kick=False + remaining_persist=KICKER_PERSIST_TIMESTEPS, + remaining_cooldown=KICKER_COOLDOWN_TIMESTEPS, + is_kick=False, ) # def _populate_robots_info(self, data_in: bytes) -> None: @@ -177,15 +186,32 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by ] ) + #### KICKER LOGIC #### + # cooldown ensures that we do not resend kick/chip command within cooldown period + # persist ensures that we resend kick/chip command for n timesteps after initial command + # embedded detects rising edge of kick/chip command only + ###################### + kicker_byte = 0 - # check kick and chip command but also check the kicker tracker to see if we need to persist the command - # elif needed here to ensure the persistance command doesnt cause us to kick and chip at the same time - if c_command.kick or (robot_id in self._kicker_tracker and self._kicker_tracker[robot_id].is_kick): + tracker = self._kicker_tracker.get(robot_id) + + # If tracker_entry exists but persistence expired → send empty kicker byte and return + if tracker and tracker.remaining_persist <= 0: + packet.append(kicker_byte) + return packet + + # Decide whether we're kicking or chipping + kick_active = c_command.kick or (tracker and tracker.remaining_persist > 0 and tracker.is_kick) + + chip_active = c_command.chip or (tracker and tracker.remaining_persist > 0 and not tracker.is_kick) + + if kick_active: kicker_byte |= 0xF0 # upper kicker full power - elif c_command.chip or (robot_id in self._kicker_tracker and not self._kicker_tracker[robot_id].is_kick): + elif chip_active: kicker_byte |= 0x0F - packet.append(kicker_byte) # Kicker controls # Frame end + packet.append(kicker_byte) + return packet # packet_str = " ".join(f"{byte:08b}" for byte in packet) From a4a63005ee10af0da9391b84ef708ec249351814 Mon Sep 17 00:00:00 2001 From: Joel Date: Sat, 24 Jan 2026 00:10:00 +0000 Subject: [PATCH 8/8] chore:cleanup --- .../src/controllers/real/real_robot_controller.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 0ce77137..c93ff98d 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -88,7 +88,7 @@ def send_robot_commands(self) -> None: if self._kicker_tracker[robot_id].remaining_persist > 0: self._kicker_tracker[robot_id].remaining_persist -= 1 else: - # reset kick command to 0 in the out_packet + # remove kicker tracker entry once cooldown is over del self._kicker_tracker[robot_id] self._out_packet = self._empty_command() # flush the out_packet @@ -211,7 +211,6 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by kicker_byte |= 0x0F packet.append(kicker_byte) - return packet # packet_str = " ".join(f"{byte:08b}" for byte in packet)