From 9981dfdea6c8adfa864d1b7f12d01af94bb3ff4e Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 16 Jan 2026 01:44:59 +0000 Subject: [PATCH 01/14] made some progress --- docker/Dockerfile | 1 + .../task_planning/interface/ivc.py | 29 +---------- .../task_planning/tasks/ivc_tasks.py | 48 ++++--------------- .../task_planning/utils/other_utils.py | 24 +++++++++- 4 files changed, 34 insertions(+), 68 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 2cd1be09..0e9a9f60 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -27,6 +27,7 @@ RUN apt install -y --no-install-recommends openssh-client RUN apt install -y --no-install-recommends python3-colcon-common-extensions RUN apt install -y --no-install-recommends python3-opencv RUN apt install -y --no-install-recommends python3-pip +RUN apt install -y --no-install-recommends python3-aiofiles RUN apt install -y --no-install-recommends python3-venv RUN apt install -y --no-install-recommends ros-jazzy-cv-bridge RUN apt install -y --no-install-recommends ros-jazzy-foxglove-bridge diff --git a/onboard/src/task_planning/task_planning/interface/ivc.py b/onboard/src/task_planning/task_planning/interface/ivc.py index 2d2752d0..f3a17247 100644 --- a/onboard/src/task_planning/task_planning/interface/ivc.py +++ b/onboard/src/task_planning/task_planning/interface/ivc.py @@ -1,8 +1,7 @@ from dataclasses import dataclass -from datetime import datetime from enum import Enum from pathlib import Path - +from datetime import datetime import pytz from custom_msgs.msg import ModemStatus, StringWithHeader from custom_msgs.srv import SendModemMessage @@ -10,8 +9,7 @@ from rclpy.node import Node from rclpy.task import Future from rclpy.time import Time -from task_planning.utils.other_utils import singleton - +from task_planning.utils.other_utils import singleton, ros_timestamp_to_pacific_time logger = get_logger('ivc_interface') class IVCMessageType(Enum): @@ -53,29 +51,6 @@ class IVCMessage: timestamp: Time msg: IVCMessageType -def ros_timestamp_to_pacific_time(sec: int, nanosec: int) -> str: - """ - Convert ROS timestamp (seconds and nanoseconds) to human-readable Pacific time. - - # TODO: move to utils + merge with same function in ivc_tasks.py - - Args: - sec (int): Seconds since epoch - nanosec (int): Nanoseconds - - Returns: - str: Human-readable timestamp in Pacific timezone - """ - # Convert to datetime object - pacific_tz = pytz.timezone('US/Pacific') - timestamp = datetime.fromtimestamp(sec + nanosec / 1e9, tz=pacific_tz) - - # Convert to Pacific timezone - pacific_time = timestamp.astimezone(pacific_tz) - - # Format as human-readable string - return pacific_time.strftime('%Y-%m-%d %H:%M:%S %Z') - @singleton class IVC: """Interface for inter-vehicle communication (IVC).""" diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index 9f390ef8..db2ca33f 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -1,13 +1,14 @@ from datetime import datetime from pathlib import Path from typing import TYPE_CHECKING, cast - +import aiofiles import pytz from rclpy.duration import Duration from rclpy.logging import get_logger from task_planning.interface.ivc import IVC, IVCMessageType from task_planning.task import Task, task from task_planning.tasks import util_tasks +from task_planning.utils.other_utils import ros_timestamp_to_pacific_time if TYPE_CHECKING: from custom_msgs.srv import SendModemMessage @@ -17,30 +18,6 @@ logger = get_logger('ivc_tasks') -def ros_timestamp_to_pacific_time(sec: int, nanosec: int) -> str: - """ - Convert ROS timestamp (seconds and nanoseconds) to human-readable Pacific time. - - # TODO: move to utils + merge with same function in ivc.py - - Args: - sec (int): Seconds since epoch - nanosec (int): Nanoseconds - - Returns: - str: Human-readable timestamp in Pacific timezone - """ - # Convert to datetime object - pacific_tz = pytz.timezone('US/Pacific') - timestamp = datetime.fromtimestamp(sec + nanosec / 1e9, tz=pacific_tz) - - # Convert to Pacific timezone - pacific_time = timestamp.astimezone(pacific_tz) - - # Format as human-readable string - return pacific_time.strftime('%Y-%m-%d %H:%M:%S %Z') - - @task async def wait_for_modem_status(self: Task[None, None, None], timeout: float = 10) -> bool: """ @@ -99,7 +76,7 @@ async def wait_for_modem_ready(self: Task[None, None, None], timeout: float = 15 logger.info('Modem is ready.') return True - +# TODO: read through this to see what's unique @task async def test_ivc(self: Task[None, None, None], msg: IVCMessageType) -> None: """Test inter-vehicle communication.""" @@ -153,7 +130,7 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM """Receive IVC message.""" await wait_for_modem_ready(parent=self) - messages_received = len(IVC().messages) + messages_received = len(IVC().messages) # TODO: This is slightly dumb. re-think if possible. sleep_task = util_tasks.sleep(timeout, parent=self) while not (len(IVC().messages) > messages_received): @@ -176,15 +153,6 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM logger.warning(f'Received message {IVC().messages[-1].msg.name} is unknown.') return IVCMessageType.UNKNOWN - -@task -async def crush_ivc_spam(self: Task[None, None, None], msg_to_send: IVCMessageType) -> Task[None, None, None]: - """Spam IVC message for Crush.""" - while True: - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - await util_tasks.sleep(20, parent=self) - - @task async def ivc_send_then_receive(self: Task[None, None, None], msg_to_send: IVCMessageType, msg_to_receive: IVCMessageType, timeout: float = 60) -> Task[None, None, None]: @@ -246,12 +214,12 @@ async def ivc_receive_then_send(self: Task[None, None, None], msg: IVCMessageTyp @task async def delineate_ivc_log(self: Task[None, None, None]) -> Task[None, None, None]: # noqa: ARG001 """Append a header to the IVC log file.""" - with Path('ivc_log.txt').open('a') as f: # noqa: ASYNC230 TODO eventually use async io - f.write('----- NEW RUN STARTED -----\n') + async with aiofiles.open('ivc_log.txt', 'a') as f: + await f.write('----- NEW RUN STARTED -----\n') @task async def add_to_ivc_log(self: Task[None, None, None], message: str) -> Task[None, None, None]: # noqa: ARG001 """Add a message to the IVC log file.""" - with Path('ivc_log.txt').open('a') as f: # noqa: ASYNC230 TODO eventually use async io - f.write(f'{message}\n') + async with aiofiles.open('ivc_log.txt', 'a') as f: + await f.write(f'{message}\n') \ No newline at end of file diff --git a/onboard/src/task_planning/task_planning/utils/other_utils.py b/onboard/src/task_planning/task_planning/utils/other_utils.py index 07e40df1..85d63361 100644 --- a/onboard/src/task_planning/task_planning/utils/other_utils.py +++ b/onboard/src/task_planning/task_planning/utils/other_utils.py @@ -2,7 +2,8 @@ from collections.abc import Callable from enum import Enum from typing import Any - +from datetime import datetime +import pytz class RobotName(Enum): """Enum for valid robot names.""" @@ -36,3 +37,24 @@ def getinstance(*args: tuple, **kwargs: dict) -> type: instances[cls] = cls(*args, **kwargs) return instances[cls] return getinstance + +def ros_timestamp_to_pacific_time(sec: int, nanosec: int) -> str: + """ + Convert ROS timestamp (seconds and nanoseconds) to human-readable Pacific time. + + Args: + sec (int): Seconds since epoch + nanosec (int): Nanoseconds + + Returns: + str: Human-readable timestamp in Pacific timezone + """ + # Convert to datetime object + pacific_tz = pytz.timezone('US/Pacific') + timestamp = datetime.fromtimestamp(sec + nanosec / 1e9, tz=pacific_tz) + + # Convert to Pacific timezone + pacific_time = timestamp.astimezone(pacific_tz) + + # Format as human-readable string + return pacific_time.strftime('%Y-%m-%d %H:%M:%S %Z') From 94271bd5d915bf2a5e5c40f015d9b3c5f9241d8f Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 30 Jan 2026 01:28:09 +0000 Subject: [PATCH 02/14] finished --- .../task_planning/tasks/ivc_tasks.py | 60 ++++--------------- 1 file changed, 11 insertions(+), 49 deletions(-) diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index db2ca33f..47358ced 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -13,9 +13,6 @@ if TYPE_CHECKING: from custom_msgs.srv import SendModemMessage -# TODO: rewrite/reorganize/clean up the comp ivc_tasks -# TODO: add docstrings for all tasks - logger = get_logger('ivc_tasks') @task @@ -76,7 +73,6 @@ async def wait_for_modem_ready(self: Task[None, None, None], timeout: float = 15 logger.info('Modem is ready.') return True -# TODO: read through this to see what's unique @task async def test_ivc(self: Task[None, None, None], msg: IVCMessageType) -> None: """Test inter-vehicle communication.""" @@ -115,12 +111,12 @@ async def ivc_send(self: Task[None, None, None], msg: IVCMessageType) -> None: logger.info(f'Sent IVC message: {msg.name}') # Log to text file - with Path('ivc_log.txt').open('a') as f: # noqa: ASYNC230 TODO switch to async IO + async with aiofiles.open('ivc_log.txt', 'a') as f: # noqa: ASYNC230 timestamp = ros_timestamp_to_pacific_time( IVC().modem_status.header.stamp.sec, IVC().modem_status.header.stamp.nanosec, ) - f.write(f'Sent IVC message: {msg.name} at {timestamp}\n') + await f.write(f'Sent IVC message: {msg.name} at {timestamp}\n') else: logger.error(f'Modem failed to send message. Response: {service_response.message}') @@ -130,7 +126,7 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM """Receive IVC message.""" await wait_for_modem_ready(parent=self) - messages_received = len(IVC().messages) # TODO: This is slightly dumb. re-think if possible. + messages_received = len(IVC().messages) sleep_task = util_tasks.sleep(timeout, parent=self) while not (len(IVC().messages) > messages_received): @@ -154,61 +150,27 @@ async def ivc_receive(self: Task[None, None, None], timeout: float = 10) -> IVCM return IVCMessageType.UNKNOWN @task -async def ivc_send_then_receive(self: Task[None, None, None], msg_to_send: IVCMessageType, - msg_to_receive: IVCMessageType, timeout: float = 60) -> Task[None, None, None]: - """Send then receive IVC message.""" - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - - count = 2 - # Wait for Oogway to say starting/acknowledge command +async def ivc_send_blocking(self: Task[None, None, None], msg_to_send: IVCMessageType, + msg_to_receive: IVCMessageType, num_attempts: int, timeout: float = 60) -> Task[None, None, None]: + """Send and wait for an IVC message in a blocking manner.""" + await ivc_send(msg_to_send, parent=self) + count = num_attempts while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive: logger.info(f'Unexpected message received. Remaining attempts: {count}') count -= 1 @task -async def crush_ivc_send(self: Task[None, None, None], msg_to_send: IVCMessageType, - msg_to_receive: IVCMessageType, timeout: float = 60) -> Task[None, None, None]: - """Send IVC message crush.""" - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - - count = 2 - # Wait for Oogway to say starting/acknowledge command - while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive: - logger.info(f'Unexpected message or no message received. Sending again. Remaining attempts: {count - 1}') - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - count -= 1 - - if count == 0: - logger.info('No acknowledgement, sending one last time') - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - - -@task -async def crush_ivc_receive(self: Task[None, None, None], msg_to_receive: IVCMessageType, - msg_to_send: IVCMessageType, timeout: float = 60) -> Task[None, None, None]: - """Receive IVC message crush.""" - count = 2 - # Wait for Oogway to say starting/acknowledge command - while count != 0 and await ivc_receive(timeout=timeout, parent=self) != msg_to_receive: - logger.info(f'Unexpected message or no message received. Remaining attempts: {count - 1}') - count -= 1 - - await ivc_send(msg_to_send, parent=self) # Send crush is done with gate - - -@task -async def ivc_receive_then_send(self: Task[None, None, None], msg: IVCMessageType, +async def ivc_receive_then_send(self: Task[None, None, None], check_msg: IVCMessageType, send_msg: IVCMessageType, timeout: float = 60) -> Task[None, None, None]: """Receive then after receipt send IVC message.""" if timeout <= 0: return - # Wait until Crush is done with gate - while await ivc_receive(timeout=timeout, parent=self) != IVCMessageType.CRUSH_GATE: + while await ivc_receive(timeout=timeout, parent=self) != check_msg: logger.info('Unexpected message received.') - await ivc_send(msg, parent=self) # Oogway says ok and starting + await ivc_send(send_msg, parent=self) @task From c0cf852e449f9558f89eaffc0f5ed5102ee5db04 Mon Sep 17 00:00:00 2001 From: Srinath Iyer Date: Fri, 30 Jan 2026 01:33:42 +0000 Subject: [PATCH 03/14] updated oogway and crush.py --- onboard/src/task_planning/task_planning/robot/crush.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index efbe15fa..d5a172a7 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -22,7 +22,7 @@ async def main(self: Task) -> Task[None, None, None]: comp_tasks.slalom_task_dead_reckoning(depth_level=0.975, parent=self), # Move through slalom via 2,2,2 # Move to octagon front via 2,2; left strafe via 0.75 comp_tasks.slalom_to_octagon_dead_reckoning(depth_level=0.975, parent=self), - ivc_tasks.crush_ivc_spam(msg_to_send=IVCMessageType.CRUSH_OCTAGON, parent=self), + ivc_tasks.ivc_send(msg=IVCMessageType.CRUSH_OCTAGON, parent=self), ######## Unused competition tasks ######## ## Gate From b49f9fa1bfb392368eb1096166148eb477d38336 Mon Sep 17 00:00:00 2001 From: Oogway Date: Thu, 19 Mar 2026 23:59:05 +0000 Subject: [PATCH 04/14] bruh --- onboard/src/task_planning/task_planning/robot/oogway.py | 8 ++++---- onboard/src/task_planning/task_planning/task.py | 4 ++++ 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/onboard/src/task_planning/task_planning/robot/oogway.py b/onboard/src/task_planning/task_planning/robot/oogway.py index cbaff9ae..1299e0de 100644 --- a/onboard/src/task_planning/task_planning/robot/oogway.py +++ b/onboard/src/task_planning/task_planning/robot/oogway.py @@ -28,10 +28,10 @@ async def main(self: Task) -> Task[None, None, None]: tasks = [ ######## Main competition tasks ######## # ivc_tasks.delineate_ivc_log(parent=self), - comp_tasks.initial_submerge(DEPTH, parent=self), + # comp_tasks.initial_submerge(DEPTH, parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=-DEPTH, parent=self), - comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH, - direction=DIRECTION_OF_TORPEDO_BANNER, parent=self), + # comp_tasks.torpedo_task(first_target=FIRST_TARGET, depth_level=DEPTH, + # direction=DIRECTION_OF_TORPEDO_BANNER, parent=self), # TODO: task not found??? # comp_tasks.send_torpedo_ivc(parent=self), # comp_tasks.octagon_task(direction=1, parent=self), @@ -74,7 +74,7 @@ async def main(self: Task) -> Task[None, None, None]: ######## Setup/Testing tasks ######## # sonar_tasks.sonar_test(-45, 45, 5, parent=self), - # ivc_tasks.test_ivc(IVCMessageType.OOGWAY_TEST, parent=self), + ivc_tasks.test_ivc(IVCMessageType.OOGWAY_TEST, parent=self), # buoyancy_tasks.buoyancy_task(-0.5, parent=self), # Submerge and stabilize buoyancy ######## Prequal tasks ######## diff --git a/onboard/src/task_planning/task_planning/task.py b/onboard/src/task_planning/task_planning/task.py index 16b93204..10fd8893 100644 --- a/onboard/src/task_planning/task_planning/task.py +++ b/onboard/src/task_planning/task_planning/task.py @@ -13,6 +13,7 @@ if TYPE_CHECKING: from rclpy.node import Node from rclpy.qos import HistoryPolicy, QoSProfile +from rclpy.task import Future from std_msgs.msg import Header from task_planning.message_conversion.jsonpickle_custom_handlers import register_custom_jsonpickle_handlers @@ -353,6 +354,9 @@ def __await__(self) -> Generator[YieldType, SendType, ReturnType]: if self._started: input_ = (yield output) output = self.send(input_) + while isinstance(output, Future) and not output.done() and not self._done: + yield output + input_ = None return output From 3174ecc1c0a2807a3adee622278e22ea05e1b068 Mon Sep 17 00:00:00 2001 From: Crush Date: Fri, 20 Mar 2026 00:34:45 +0000 Subject: [PATCH 05/14] some changes --- docker/Dockerfile | 1 + .../task_planning/interface/ivc.py | 2 +- .../task_planning/robot/crush.py | 21 ++++++++++--------- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index 0e9a9f60..75989f46 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -29,6 +29,7 @@ RUN apt install -y --no-install-recommends python3-opencv RUN apt install -y --no-install-recommends python3-pip RUN apt install -y --no-install-recommends python3-aiofiles RUN apt install -y --no-install-recommends python3-venv +RUN apt install -y --no-install-recommends python3-pytz RUN apt install -y --no-install-recommends ros-jazzy-cv-bridge RUN apt install -y --no-install-recommends ros-jazzy-foxglove-bridge RUN apt install -y --no-install-recommends ros-jazzy-robot-localization diff --git a/onboard/src/task_planning/task_planning/interface/ivc.py b/onboard/src/task_planning/task_planning/interface/ivc.py index f3a17247..bdaeff76 100644 --- a/onboard/src/task_planning/task_planning/interface/ivc.py +++ b/onboard/src/task_planning/task_planning/interface/ivc.py @@ -29,10 +29,10 @@ class IVCMessageType(Enum): """ UNKNOWN = '' OOGWAY_TEST = 'to' + CRUSH_TEST = 'tc' OOGWAY_GATE = 'og' OOGWAY_ACKNOWLEDGE = 'oa' OOGWAY_TORPEDOES = 'fu' - CRUSH_TEST = 'tc' CRUSH_GATE = 'cg' CRUSH_ACKNOWLEDGE = 'ca' CRUSH_OCTAGON = 'ky' diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index d5a172a7..1b3e59d6 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -14,16 +14,17 @@ async def main(self: Task) -> Task[None, None, None]: """Run the tasks to be performed by Crush.""" tasks = [ ######## Main competition tasks ######## - ivc_tasks.delineate_ivc_log(parent=self), - comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=True, timeout=10, parent=self), - comp_tasks.coin_flip(enable_same_direction=False, parent=self), - comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 - comp_tasks.gate_style_task(depth_level=0.975, parent=self), # Spin - comp_tasks.slalom_task_dead_reckoning(depth_level=0.975, parent=self), # Move through slalom via 2,2,2 - # Move to octagon front via 2,2; left strafe via 0.75 - comp_tasks.slalom_to_octagon_dead_reckoning(depth_level=0.975, parent=self), - ivc_tasks.ivc_send(msg=IVCMessageType.CRUSH_OCTAGON, parent=self), - + # ivc_tasks.delineate_ivc_log(parent=self), + # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=True, timeout=10, parent=self), + # comp_tasks.coin_flip(enable_same_direction=False, parent=self), + # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 + # comp_tasks.gate_style_task(depth_level=0.975, parent=self), # Spin + # comp_tasks.slalom_task_dead_reckoning(depth_level=0.975, parent=self), # Move through slalom via 2,2,2 + # # Move to octagon front via 2,2; left strafe via 0.75 + # comp_tasks.slalom_to_octagon_dead_reckoning(depth_level=0.975, parent=self), + # ivc_tasks.ivc_send(msg=IVCMessageType.CRUSH_OCTAGON, parent=self), + ivc_tasks.wait_for_modem_ready(timeout=30, parent=self), + ivc_tasks.test_ivc(IVCMessageType.CRUSH_TEST, parent=self), ######## Unused competition tasks ######## ## Gate # comp_tasks.gate_task(offset=-0.1, direction=-1, parent=self), From 29f947043bb4fae403e49f5c711a661770f83a5e Mon Sep 17 00:00:00 2001 From: Oogway Date: Sat, 11 Jul 2026 06:10:06 +0000 Subject: [PATCH 06/14] made linter happy --- onboard/src/task_planning/task_planning/tasks/ivc_tasks.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index 47358ced..1273cbbe 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -184,4 +184,4 @@ async def delineate_ivc_log(self: Task[None, None, None]) -> Task[None, None, No async def add_to_ivc_log(self: Task[None, None, None], message: str) -> Task[None, None, None]: # noqa: ARG001 """Add a message to the IVC log file.""" async with aiofiles.open('ivc_log.txt', 'a') as f: - await f.write(f'{message}\n') \ No newline at end of file + await f.write(f'{message}\n') From 80c15b28167f6e22faf1bbf977933c203a9ed3bb Mon Sep 17 00:00:00 2001 From: Oogway Date: Sat, 11 Jul 2026 04:20:21 -0400 Subject: [PATCH 07/14] some changes --- docker/Dockerfile | 8 ++++---- onboard/src/task_planning/task_planning/interface/ivc.py | 2 +- onboard/src/task_planning/task_planning/robot/oogway.py | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/docker/Dockerfile b/docker/Dockerfile index ff8fadac..e79dc5c1 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,9 +1,10 @@ + # Base Docker image for ROS2 Jazzy that supports linux/amd64 and linux/arm64 FROM ros:jazzy # Dummy argument to force Docker to rebuild the image by invalidating the cache when the argument value changes # Cache invalidation forces Docker to rerun the `apt update` command, ensuring that the latest `apt` package index is used when building the image -ARG CACHE_BUSTER +# ARG CACHE_BUSTER # ********************************************************************************************************************************************************************************* # All commands below are run as root @@ -27,9 +28,8 @@ RUN apt install -y --no-install-recommends openssh-client RUN apt install -y --no-install-recommends python3-colcon-common-extensions RUN apt install -y --no-install-recommends python3-opencv RUN apt install -y --no-install-recommends python3-pip -RUN apt install -y --no-install-recommends python3-aiofiles RUN apt install -y --no-install-recommends python3-venv -RUN apt install -y --no-install-recommends python3-pytz +RUN apt install -y --no-install-recommends python3-aiofiles RUN apt install -y --no-install-recommends ros-jazzy-cv-bridge RUN apt install -y --no-install-recommends ros-jazzy-foxglove-bridge RUN apt install -y --no-install-recommends ros-jazzy-robot-localization @@ -211,4 +211,4 @@ RUN --mount=type=secret,id=foxgloverc \ fi # Run entrypoint script via tini to ensure SIGTERM is properly handled and zombie processes are reaped -ENTRYPOINT ["/usr/bin/tini", "--", "/usr/local/bin/entrypoint.sh"] +ENTRYPOINT ["/usr/bin/tini", "--", "/usr/local/bin/entrypoint.sh"] \ No newline at end of file diff --git a/onboard/src/task_planning/task_planning/interface/ivc.py b/onboard/src/task_planning/task_planning/interface/ivc.py index 81ef11dc..afcd5bfb 100644 --- a/onboard/src/task_planning/task_planning/interface/ivc.py +++ b/onboard/src/task_planning/task_planning/interface/ivc.py @@ -163,7 +163,7 @@ def _on_receive_modem_message(self, msg: StringWithHeader) -> None: seconds, nanoseconds, ) - msg = f'Received IVC message: {ivc_message.msg.name} at {timestamp}\n' + msg = f'Received IVC message: {ivc_message.msg.name} at {timestamp}. Raw data is {msg}.\n' # Log to text file logger.info(msg) diff --git a/onboard/src/task_planning/task_planning/robot/oogway.py b/onboard/src/task_planning/task_planning/robot/oogway.py index 655afd84..01ebb10d 100644 --- a/onboard/src/task_planning/task_planning/robot/oogway.py +++ b/onboard/src/task_planning/task_planning/robot/oogway.py @@ -45,7 +45,7 @@ async def main(self: Task) -> Task[None, None, None]: ######## Unused competition tasks ######## ## Coin flip - comp_tasks.coin_flip(parent=self), + #comp_tasks.coin_flip(parent=self), ## Gate # comp_tasks.gate_task(offset=-0.1, direction=-1, parent=self), From 2c102b8c58c409328c718d4beb05e5cb28bc021a Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Sat, 11 Jul 2026 10:02:39 -0700 Subject: [PATCH 08/14] IVC tests with aiofiles --- .../task_planning/robot/crush.py | 11 +++--- .../task_planning/tasks/ivc_tasks.py | 36 +++++++++---------- 2 files changed, 24 insertions(+), 23 deletions(-) diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index c37d569a..f07453cd 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -3,23 +3,24 @@ import numpy as np from task_planning.interface.cv import CVObjectType -# from task_planning.interface.ivc import IVCMessageType +from task_planning.interface.ivc import IVCMessageType from task_planning.task import Task, task -from task_planning.tasks import buoyancy_tasks, comp_tasks, move_tasks, prequal_tasks, sonar_tasks +from task_planning.tasks import buoyancy_tasks, comp_tasks, move_tasks, prequal_tasks, sonar_tasks, ivc_tasks from task_planning.utils import geometry_utils + @task async def main(self: Task) -> Task[None, None, None]: """Run the tasks to be performed by Crush.""" tasks = [ ######## Main competition tasks ######## # ivc_tasks.delineate_ivc_log(parent=self), - comp_tasks.initial_submerge(0.5, parent=self), + # comp_tasks.initial_submerge(0.5, parent=self), # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self), # comp_tasks.coin_flip(parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 - comp_tasks.gate_style_task(depth_level=0.7, parent=self), # Spin + # comp_tasks.gate_style_task(depth_level=0.7, parent=self), # Spin # comp_tasks.slalom_task_dead_reckoning(depth_level=0.975, parent=self), # Move through slalom via 2,2,2 # Move to octagon front via 2,2; left strafe via 0.75 # comp_tasks.slalom_to_octagon_dead_reckoning(depth_level=0.975, parent=self), @@ -50,7 +51,7 @@ async def main(self: Task) -> Task[None, None, None]: # msg_to_receive=IVCMessageType.OOGWAY_ACKNOWLEDGE, timeout=90, parent=self), # ivc_tasks.crush_ivc_receive(msg_to_receive=IVCMessageType.OOGWAY_GATE, # msg_to_send=IVCMessageType.CRUSH_ACKNOWLEDGE, timeout=90, parent=self), - + ivc_tasks.test_ivc(IVCMessageType.CRUSH_TEST, parent=self), ## Movement/CV tasks # move_tasks.yaw_from_local_pose(np.pi / 2, parent=self), # move_tasks.move_with_directions([(1, 0, 0)], depth_level=-0.7, correct_depth=True, correct_yaw=True, diff --git a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py index 1273cbbe..a535cf9b 100644 --- a/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/ivc_tasks.py @@ -1,7 +1,7 @@ from datetime import datetime from pathlib import Path from typing import TYPE_CHECKING, cast -import aiofiles +# import aiofiles import pytz from rclpy.duration import Duration from rclpy.logging import get_logger @@ -110,13 +110,13 @@ async def ivc_send(self: Task[None, None, None], msg: IVCMessageType) -> None: if service_response.success: logger.info(f'Sent IVC message: {msg.name}') - # Log to text file - async with aiofiles.open('ivc_log.txt', 'a') as f: # noqa: ASYNC230 - timestamp = ros_timestamp_to_pacific_time( - IVC().modem_status.header.stamp.sec, - IVC().modem_status.header.stamp.nanosec, - ) - await f.write(f'Sent IVC message: {msg.name} at {timestamp}\n') + # # Log to text file + # async with aiofiles.open('ivc_log.txt', 'a') as f: # noqa: ASYNC230 + # timestamp = ros_timestamp_to_pacific_time( + # IVC().modem_status.header.stamp.sec, + # IVC().modem_status.header.stamp.nanosec, + # ) + # await f.write(f'Sent IVC message: {msg.name} at {timestamp}\n') else: logger.error(f'Modem failed to send message. Response: {service_response.message}') @@ -173,15 +173,15 @@ async def ivc_receive_then_send(self: Task[None, None, None], check_msg: IVCMess await ivc_send(send_msg, parent=self) -@task -async def delineate_ivc_log(self: Task[None, None, None]) -> Task[None, None, None]: # noqa: ARG001 - """Append a header to the IVC log file.""" - async with aiofiles.open('ivc_log.txt', 'a') as f: - await f.write('----- NEW RUN STARTED -----\n') +# @task +# async def delineate_ivc_log(self: Task[None, None, None]) -> Task[None, None, None]: # noqa: ARG001 +# """Append a header to the IVC log file.""" +# async with aiofiles.open('ivc_log.txt', 'a') as f: +# await f.write('----- NEW RUN STARTED -----\n') -@task -async def add_to_ivc_log(self: Task[None, None, None], message: str) -> Task[None, None, None]: # noqa: ARG001 - """Add a message to the IVC log file.""" - async with aiofiles.open('ivc_log.txt', 'a') as f: - await f.write(f'{message}\n') +# @task +# async def add_to_ivc_log(self: Task[None, None, None], message: str) -> Task[None, None, None]: # noqa: ARG001 +# """Add a message to the IVC log file.""" +# async with aiofiles.open('ivc_log.txt', 'a') as f: +# await f.write(f'{message}\n') From 09244cd7700cac480a379fde6a97fcad24343024 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Sun, 12 Jul 2026 02:53:58 -0700 Subject: [PATCH 09/14] DO NOT TOUCH THIS CONFIG.YAML OR SAAGAR WILL TOUCH UOU --- docker/ros_bashrc.sh | 3 +++ onboard/src/offboard_comms/config/crush.yaml | 10 +++++----- onboard/src/task_planning/task_planning/robot/crush.py | 7 +++---- onboard/src/task_planning/task_planning/task_runner.py | 2 +- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/docker/ros_bashrc.sh b/docker/ros_bashrc.sh index b7ff48e7..939228c6 100755 --- a/docker/ros_bashrc.sh +++ b/docker/ros_bashrc.sh @@ -62,3 +62,6 @@ source /home/ubuntu/robosub-ros2/venv.sh activate # Get seconds since epoch alias secs="date +%s" +hat is not random garbage—that is the standard ASCII ACK (Acknowledge) control character. + +In serial communication, hardware devices often send an ACK (\x06) to confirm they successfully received and processed a comm \ No newline at end of file diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 86992644..16716626 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -57,20 +57,20 @@ thrusters: cw: false standard_esc: true - name: middle_right_second - cw: true - standard_esc: false + cw: false + standard_esc: true - name: back_right cw: false - standard_esc: false + standard_esc: true - name: back_left cw: true - standard_esc: false + standard_esc: true - name: middle_left_first cw: false standard_esc: true - name: middle_left_second cw: true - standard_esc: false + standard_esc: true - name: front_left cw: true standard_esc: true diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index f07453cd..6a3f4bd0 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -7,7 +7,7 @@ from task_planning.task import Task, task from task_planning.tasks import buoyancy_tasks, comp_tasks, move_tasks, prequal_tasks, sonar_tasks, ivc_tasks from task_planning.utils import geometry_utils - +import time @task @@ -16,7 +16,7 @@ async def main(self: Task) -> Task[None, None, None]: tasks = [ ######## Main competition tasks ######## # ivc_tasks.delineate_ivc_log(parent=self), - # comp_tasks.initial_submerge(0.5, parent=self), + comp_tasks.initial_submerge(0.5, parent=self), # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self), # comp_tasks.coin_flip(parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 @@ -51,7 +51,7 @@ async def main(self: Task) -> Task[None, None, None]: # msg_to_receive=IVCMessageType.OOGWAY_ACKNOWLEDGE, timeout=90, parent=self), # ivc_tasks.crush_ivc_receive(msg_to_receive=IVCMessageType.OOGWAY_GATE, # msg_to_send=IVCMessageType.CRUSH_ACKNOWLEDGE, timeout=90, parent=self), - ivc_tasks.test_ivc(IVCMessageType.CRUSH_TEST, parent=self), + #ivc_tasks.test_ivc(IVCMessageType.CRUSH_TEST, parent=self), ## Movement/CV tasks # move_tasks.yaw_from_local_pose(np.pi / 2, parent=self), # move_tasks.move_with_directions([(1, 0, 0)], depth_level=-0.7, correct_depth=True, correct_yaw=True, @@ -72,6 +72,5 @@ async def main(self: Task) -> Task[None, None, None]: ######## Prequal tasks ######## # prequal_tasks.prequal_task(parent=self), ] - for task_to_run in tasks: await task_to_run diff --git a/onboard/src/task_planning/task_planning/task_runner.py b/onboard/src/task_planning/task_planning/task_runner.py index d7cd5abb..0266e4dd 100644 --- a/onboard/src/task_planning/task_planning/task_runner.py +++ b/onboard/src/task_planning/task_planning/task_runner.py @@ -47,7 +47,7 @@ def __init__(self) -> None: CV(self, bypass=self.bypass) IVC(node=self, bypass=self.bypass) Servos(self, bypass=self.bypass) - Sonar(self, bypass=self.bypass) + #Sonar(self, bypass=self.bypass) State(self, tf_buffer=tf_buffer, bypass=self.bypass) # Initialize the task update publisher From 265f3cfe4d8ddf1a5c8da6b6aec341524da871b1 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Sun, 12 Jul 2026 08:38:40 -0700 Subject: [PATCH 10/14] fixes to ros_bashrc.sh --- docker/ros_bashrc.sh | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/docker/ros_bashrc.sh b/docker/ros_bashrc.sh index 939228c6..5fe0b26a 100755 --- a/docker/ros_bashrc.sh +++ b/docker/ros_bashrc.sh @@ -31,6 +31,12 @@ fi export _colcon_cd_root=/opt/ros/jazzy +alias launch-robot="ros2 launch execute robot.launch.py" + +alias tp-run="ros2 run task_planning task_runner" + +alias tp-run-auto="ros2 run task_planning task_runner --ros-args autonomous:=True" + # Alias to start foxglove bridge alias fg-ws="ros2 launch foxglove_bridge foxglove_bridge_launch.xml port:=28765" @@ -61,7 +67,4 @@ nvm use lts/jod > /dev/null # Suppress stdout, only show stderr source /home/ubuntu/robosub-ros2/venv.sh activate # Get seconds since epoch -alias secs="date +%s" -hat is not random garbage—that is the standard ASCII ACK (Acknowledge) control character. - -In serial communication, hardware devices often send an ACK (\x06) to confirm they successfully received and processed a comm \ No newline at end of file +alias secs="date +%s" \ No newline at end of file From e06bf50fd83e6e3cec652ae570bf2d7beb1bf290 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Sun, 12 Jul 2026 23:29:06 -0700 Subject: [PATCH 11/14] fully tuned crush's thruster configs --- onboard/src/offboard_comms/config/crush.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 16716626..35f157fe 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -54,7 +54,7 @@ thrusters: cw: false standard_esc: true - name: middle_right_first - cw: false + cw: true standard_esc: true - name: middle_right_second cw: false From 5c5feb459f891fc7e72e29642a6c0493a1495336 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Mon, 13 Jul 2026 00:15:32 -0700 Subject: [PATCH 12/14] yaw --- onboard/src/controls/config/crush.yaml | 24 +++++++++---------- .../task_planning/robot/crush.py | 9 ++++++- .../task_planning/task_runner.py | 4 ++-- 3 files changed, 21 insertions(+), 16 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index 57f83609..c35fd379 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -1,9 +1,9 @@ pid: position: x: - Kp: 0.85 + Kp: 0.84999999999999998 Ki: 0 - Kd: 0.40 + Kd: 0.40000000000000002 Ff: 0 control_effort: min: -1 @@ -11,9 +11,9 @@ pid: derivative_type: 1 error_ramp_rate: 1000 y: - Kp: 0.80 + Kp: 0.80000000000000004 Ki: 0 - Kd: 0.05 + Kd: 0.050000000000000003 Ff: 0 control_effort: min: -1 @@ -21,7 +21,7 @@ pid: derivative_type: 1 error_ramp_rate: 1000 z: - Kp: 1.43 + Kp: 1.4299999999999999 Ki: 0 Kd: 0.5 Ff: 0 @@ -31,7 +31,7 @@ pid: derivative_type: 1 error_ramp_rate: 1000 roll: - Kp: 0.05 + Kp: 0.050000000000000003 Ki: 0 Kd: 0 Ff: 0 @@ -41,7 +41,7 @@ pid: derivative_type: 1 error_ramp_rate: 1000 pitch: - Kp: 0.05 + Kp: 0.050000000000000003 Ki: 0 Kd: 0 Ff: 0 @@ -51,9 +51,9 @@ pid: derivative_type: 1 error_ramp_rate: 1000 yaw: - Kp: 0.09 + Kp: 0.16 Ki: 0 - Kd: 0.02 + Kd: 0.12 Ff: 0 control_effort: min: -1 @@ -135,7 +135,7 @@ pid: y: Kp: 1 Ki: 0.02 - Kd: 0.05 + Kd: 0.050000000000000003 Ff: 0 control_effort: min: -1 @@ -206,8 +206,6 @@ static_power_global: y: 0 z: -0.206 power_scale_factor: 1 -# all x positions assume the corner link is 10.41527MM +x than it actually is -# see README.md for more details thrusters: - name: front_right type: T200 @@ -242,4 +240,4 @@ thrusters: pos: [0.001895, 0.09596, -0.003175] rpy: [0, 0, 135] wrench_matrix_file_path: data/crush_wrench.csv -wrench_matrix_pinv_file_path: data/crush_wrench_pinv.csv +wrench_matrix_pinv_file_path: data/crush_wrench_pinv.csv \ No newline at end of file diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index 6a3f4bd0..83bdcdd6 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -16,7 +16,14 @@ async def main(self: Task) -> Task[None, None, None]: tasks = [ ######## Main competition tasks ######## # ivc_tasks.delineate_ivc_log(parent=self), - comp_tasks.initial_submerge(0.5, parent=self), + comp_tasks.initial_submerge(0.35, parent=self), + # move_tasks.move_with_directions([(1, 0, 0)], parent=self), + move_tasks.move_to_pose_local(geometry_utils.create_pose(0, 0, 0, 0, 0, np.pi/2), + pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), + parent=self), + move_tasks.move_to_pose_local(geometry_utils.create_pose(0, 0, 0, 0, 0, -np.pi), + pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), + parent=self), # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self), # comp_tasks.coin_flip(parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 diff --git a/onboard/src/task_planning/task_planning/task_runner.py b/onboard/src/task_planning/task_planning/task_runner.py index 0266e4dd..589023a3 100644 --- a/onboard/src/task_planning/task_planning/task_runner.py +++ b/onboard/src/task_planning/task_planning/task_runner.py @@ -45,9 +45,9 @@ def __init__(self) -> None: # Initialize interfaces Controls(self, bypass=self.bypass) CV(self, bypass=self.bypass) - IVC(node=self, bypass=self.bypass) + # IVC(node=self, bypass=self.bypass) Servos(self, bypass=self.bypass) - #Sonar(self, bypass=self.bypass) + # Sonar(self, bypass=self.bypass) State(self, tf_buffer=tf_buffer, bypass=self.bypass) # Initialize the task update publisher From d0b150af6c4d1388783bf97a23404af0da2e69c6 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Mon, 13 Jul 2026 00:27:40 -0700 Subject: [PATCH 13/14] x --- onboard/src/controls/config/crush.yaml | 4 ++-- onboard/src/task_planning/task_planning/robot/crush.py | 8 ++++---- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index c35fd379..f03b4a7e 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -1,9 +1,9 @@ pid: position: x: - Kp: 0.84999999999999998 + Kp: 0.80000000000000004 Ki: 0 - Kd: 0.40000000000000002 + Kd: 0.41999999999999998 Ff: 0 control_effort: min: -1 diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index 83bdcdd6..b9390eab 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -18,12 +18,12 @@ async def main(self: Task) -> Task[None, None, None]: # ivc_tasks.delineate_ivc_log(parent=self), comp_tasks.initial_submerge(0.35, parent=self), # move_tasks.move_with_directions([(1, 0, 0)], parent=self), - move_tasks.move_to_pose_local(geometry_utils.create_pose(0, 0, 0, 0, 0, np.pi/2), + move_tasks.move_to_pose_local(geometry_utils.create_pose(2, 0, 0, 0, 0, 0), pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), - parent=self), - move_tasks.move_to_pose_local(geometry_utils.create_pose(0, 0, 0, 0, 0, -np.pi), + parent=self, keep_orientation=True), + move_tasks.move_to_pose_local(geometry_utils.create_pose(-2, 0, 0, 0, 0, 0), pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), - parent=self), + parent=self, keep_orientation=True), # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self), # comp_tasks.coin_flip(parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501 From a877a7a87b3251c5f778a2bbae1d94e97099a8a1 Mon Sep 17 00:00:00 2001 From: duke-robotics-crush Date: Mon, 13 Jul 2026 01:08:22 -0700 Subject: [PATCH 14/14] y --- onboard/src/controls/config/crush.yaml | 6 +++--- onboard/src/task_planning/task_planning/robot/crush.py | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index f03b4a7e..1921ea23 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -11,9 +11,9 @@ pid: derivative_type: 1 error_ramp_rate: 1000 y: - Kp: 0.80000000000000004 + Kp: 0.84999999999999998 Ki: 0 - Kd: 0.050000000000000003 + Kd: 0.34999999999999998 Ff: 0 control_effort: min: -1 @@ -23,7 +23,7 @@ pid: z: Kp: 1.4299999999999999 Ki: 0 - Kd: 0.5 + Kd: 0.84999999999999998 Ff: 0 control_effort: min: -1 diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index b9390eab..be13f4da 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -20,10 +20,10 @@ async def main(self: Task) -> Task[None, None, None]: # move_tasks.move_with_directions([(1, 0, 0)], parent=self), move_tasks.move_to_pose_local(geometry_utils.create_pose(2, 0, 0, 0, 0, 0), pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), - parent=self, keep_orientation=True), - move_tasks.move_to_pose_local(geometry_utils.create_pose(-2, 0, 0, 0, 0, 0), + parent=self), + move_tasks.move_to_pose_local(geometry_utils.create_pose(2, 0, 0, 0, 0, 0), pose_tolerances=move_tasks.create_twist_tolerance(angular_yaw=0.3), - parent=self, keep_orientation=True), + parent=self), # comp_tasks.initial_submerge(0.5, z_tolerance=0.15, enable_controls_flag=False, timeout=10, parent=self), # comp_tasks.coin_flip(parent=self), # comp_tasks.gate_task_dead_reckoning(depth_level=0.7, parent=self), # Move through gate via 2,2; right strafe via 1.5 # noqa: E501