From ba47fcaaddbc9ece4022af8f2579bb6fac333518 Mon Sep 17 00:00:00 2001 From: Alec <116045916+alec-03@users.noreply.github.com> Date: Fri, 14 Nov 2025 22:42:06 -0500 Subject: [PATCH 01/19] Update crush.yaml Updated crush YAML for 8 thrusters. Assumes the corner link is 10.4mm forwards (+x) of the measurement point so that we don't have to update DVL reference. May me important later lol. --- onboard/src/controls/config/crush.yaml | 25 +++++++++++++++++-------- 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index 7ebf7ec0..f5804819 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -206,30 +206,39 @@ 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 thrusters: - - name: middle_right + - name: middle_right_first type: T200 - pos: [-0.2404152701, -0.42425, -0.003175] + pos: [-0.15501, -0.34805, -0.003175] + rpy: [0, 90, 0] + - name: middle_right_second + type: T200 + pos: [-0.32582, -0.34805, -0.003175] rpy: [0, 90, 0] - name: front_right type: T200 - pos: [3.360246e-05, -0.3867988726, -0.003175] + pos: [0.001895, -0.38866, -0.003175] rpy: [0, 0, -135] - name: back_right type: T200 - pos: [-0.4808641427, -0.3867988726, -0.003175] + pos: [-0.48273, -0.38866, -0.003175] rpy: [0, 0, 135] - name: back_left type: T200 - pos: [-0.4808641427, 0.09409887258, -0.003175] + pos: [-0.48273, 0.09596, -0.003175] rpy: [0, 0, -135] - - name: middle_left + - name: middle_left_first + type: T200 + pos: [-0.15501, 0.05535, -0.003175] + rpy: [0, 90, 0] + - name: middle_left_second type: T200 - pos: [-0.2404152701, 0.13155, -0.003175] + pos: [-0.32582, 0.05535, -0.003175] rpy: [0, 90, 0] - name: front_left type: T200 - pos: [3.360246e-05, 0.09409887258, -0.003175] + 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 From 7f2402f3c6b419e7346f49afe44cca7d6900ed71 Mon Sep 17 00:00:00 2001 From: Alec <116045916+alec-03@users.noreply.github.com> Date: Fri, 14 Nov 2025 22:53:40 -0500 Subject: [PATCH 02/19] Update crush_wrench.csv #93 computed wrench matrix with new thrusters --- onboard/src/controls/data/crush_wrench.csv | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/onboard/src/controls/data/crush_wrench.csv b/onboard/src/controls/data/crush_wrench.csv index 653b9f31..11200e2f 100644 --- a/onboard/src/controls/data/crush_wrench.csv +++ b/onboard/src/controls/data/crush_wrench.csv @@ -1,6 +1,6 @@ -0.0,-0.7071067812,-0.7071067812,-0.7071067812,0.0,-0.7071067812 -0.0,-0.7071067812,0.7071067812,-0.7071067812,0.0,0.7071067812 --1.0,0.0,0.0,0.0,-1.0,0.0 -0.2779,0.0104651804,-0.0104651804,0.0104651804,-0.2779,-0.0104651804 -0.0,-0.0104651804,-0.0104651804,-0.0104651804,0.0,-0.0104651804 -0.0,-0.3400460567,-0.3400460567,0.3400460567,0.0,0.3400460567 \ No newline at end of file +0.0,0.0,-0.7071067812,-0.7071067812,-0.7071067812,0.0,0.0,-0.7071067812 +0.0,0.0,-0.7071067812,0.7071067812,-0.7071067812,0.0,0.0,0.7071067812 +-1.0,-1.0,0.0,0.0,0.0,-1.0,-1.0,0.0 +0.6576125,0.6576125,0.1234785217,-0.1234785217,0.1234785217,0.2542125,0.2542125,-0.1234785217 +0.0754315,-0.0953785,-0.1234785217,-0.1234785217,-0.1234785217,0.0754315,-0.0953785,-0.1234785217 +0.0,0.0,-0.6580045792,-0.6721127737,0.0273551329,0.0,0.0,0.0132469384 From 8367bca71e519c4290e979e6d49d99c433f2a2f9 Mon Sep 17 00:00:00 2001 From: Alec <116045916+alec-03@users.noreply.github.com> Date: Fri, 14 Nov 2025 22:54:30 -0500 Subject: [PATCH 03/19] Update crush_wrench_pinv.csv #93 computed pinv for new thruster setup --- onboard/src/controls/data/crush_wrench_pinv.csv | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/onboard/src/controls/data/crush_wrench_pinv.csv b/onboard/src/controls/data/crush_wrench_pinv.csv index cda5234b..01800689 100644 --- a/onboard/src/controls/data/crush_wrench_pinv.csv +++ b/onboard/src/controls/data/crush_wrench_pinv.csv @@ -1,6 +1,8 @@ -0.0,0.0266282836,-0.5,1.7992083483,0.0,0.0 --0.3534759652,-0.3535533906,0.0,0.0,-0.0052314443,-0.7351945276 --0.3534759652,0.3535533906,0.0,0.0,-0.0052314443,-0.7351945276 --0.3534759652,-0.3535533906,0.0,0.0,-0.0052314443,0.7351945276 -0.0,-0.0266282836,-0.5,-1.7992083483,0.0,0.0 --0.3534759652,0.3535533906,0.0,0.0,-0.0052314443,0.7351945276 \ No newline at end of file +-0.511167379,0.2164414973,0.285892663,1.2394645513,2.9272290849,0.0 +0.511167379,0.2164414973,0.3442821015,1.2394645513,-2.9272290849,0.0 +-0.0209452055,-0.3608313206,0.0,0.0,0.0,-0.729543904 +-0.0209452055,0.3462754606,0.0,0.0,0.0,-0.729543904 +-0.6861615757,-0.3462754606,0.0,0.0,0.0,0.729543904 +-0.511167379,-0.2164414973,-0.8442821015,-1.2394645513,2.9272290849,0.0 +0.511167379,-0.2164414973,-0.785892663,-1.2394645513,-2.9272290849,0.0 +-0.6861615757,0.3608313206,0.0,0.0,0.0,0.729543904 From 3f204cd7253ed201c48be33705cecc38ea8dc930 Mon Sep 17 00:00:00 2001 From: Crush Date: Fri, 21 Nov 2025 01:59:23 +0000 Subject: [PATCH 04/19] Add 8 thrusters to crush offboard_comms --- onboard/src/offboard_comms/config/crush.yaml | 10 ++++++++-- .../src/offboard_comms/sketches/thruster/thruster.ino | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 9078fe22..51a456b2 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -38,7 +38,10 @@ gyro: modem: ftdi: CP2102 thrusters: - - name: middle_right + - name: middle_right_first # TODO: Validate cw and standard_esc values for all middle_left and middle_right thrusters + cw: false + standard_esc: false + - name: middle_right_second cw: false standard_esc: false - name: front_right @@ -50,7 +53,10 @@ thrusters: - name: back_left cw: true standard_esc: false - - name: middle_left + - name: middle_left_first + cw: true + standard_esc: true + - name: middle_left_second cw: true standard_esc: true - name: front_left diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index 087e7a4e..ab038b87 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -55,7 +55,7 @@ void setup() { THRUSTER_PWM_OFFSET = 57; break; case CRUSH: - NUM_THRUSTERS = 6; + NUM_THRUSTERS = 8; THRUSTER_PWM_OFFSET = 63; break; default: From 1c69b781c6e82b16b4e3416e0dd49b461d3b8acf Mon Sep 17 00:00:00 2001 From: Alec Henderson Date: Sat, 22 Nov 2025 19:54:30 +0000 Subject: [PATCH 05/19] test --- onboard/src/task_planning/task_planning/tasks/prequal_tasks.py | 1 + 1 file changed, 1 insertion(+) diff --git a/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py b/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py index b75c5d9c..9f7014fa 100644 --- a/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py @@ -16,6 +16,7 @@ LANE_MARKER_HEIGHT_METERS = 0.3048 + @task async def prequal_task(self: Task) -> Task[None, None, None]: # noqa: PLR0915 """Complete the prequalification task by tracking the lane marker.""" From a4191d3fc0dcc39f3eda8e8a8a7237f88089739d Mon Sep 17 00:00:00 2001 From: Crush Date: Fri, 5 Dec 2025 01:35:24 +0000 Subject: [PATCH 06/19] added ability to test individual thrusters --- .../offboard_comms/offboard_comms/test_thrusters.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/onboard/src/offboard_comms/offboard_comms/test_thrusters.py b/onboard/src/offboard_comms/offboard_comms/test_thrusters.py index 2cddef32..8c814d23 100644 --- a/onboard/src/offboard_comms/offboard_comms/test_thrusters.py +++ b/onboard/src/offboard_comms/offboard_comms/test_thrusters.py @@ -32,7 +32,7 @@ class ThrusterTester(Node): CONFIG_PATH = f'package://controls/config/{os.getenv("ROBOT_NAME")}.yaml' - def __init__(self, speed: float, rate: float, log_allocs: bool) -> None: + def __init__(self, speed: float, rate: float, log_allocs: bool, tTestNum: int) -> None: """ Initialize the thruster tester node. @@ -46,7 +46,13 @@ def __init__(self, speed: float, rate: float, log_allocs: bool) -> None: self.publisher_ = self.create_publisher(ThrusterAllocs, '/controls/thruster_allocs', 10) self.num_thrusters = self.get_thruster_count() self.thrust_speed = speed - self.allocs = [self.thrust_speed] * self.num_thrusters + if (tTestNum == 0): + self.allocs = [self.thrust_speed] * self.num_thrusters + else: + sArr = [0.0]*self.num_thrusters + sArr[tTestNum - 1] = speed + self.allocs = sArr + self.msg = ThrusterAllocs() self.msg.allocs = self.allocs self.log_allocs = log_allocs @@ -86,10 +92,11 @@ def main(args: list[str] | None = None) -> None: parser.add_argument('-r', '--rate', type=float, default=DEFAULT_RATE, help=f'Publishing rate in Hz (default: {DEFAULT_RATE}).') parser.add_argument('--log-allocs', action='store_true', help='Log each thruster allocs message published.') + parser.add_argument('-t', '--test-num', type=int, default=0, help='Thruster number to test (default: 0 for all thrusters).') parsed_args = parser.parse_args() rclpy.init(args=args) - node = ThrusterTester(parsed_args.speed, parsed_args.rate, parsed_args.log_allocs) + node = ThrusterTester(parsed_args.speed, parsed_args.rate, parsed_args.log_allocs, parsed_args.test_num) try: rclpy.spin(node) except KeyboardInterrupt: From f9bc8b806e484970900e74a2151871cbc082349a Mon Sep 17 00:00:00 2001 From: Crush Date: Sat, 31 Jan 2026 12:21:59 -0800 Subject: [PATCH 07/19] updated mux library for new servo driver - 8 thrusters spins --- onboard/src/cv/config/crush.yaml | 4 +- onboard/src/offboard_comms/config/crush.yaml | 2 + .../thruster/Adafruit_PWMServoDriver.cpp | 426 +++++++++++++----- .../thruster/Adafruit_PWMServoDriver.h | 147 +++--- .../sketches/thruster/thruster.ino | 12 +- 5 files changed, 408 insertions(+), 183 deletions(-) diff --git a/onboard/src/cv/config/crush.yaml b/onboard/src/cv/config/crush.yaml index 0c23836a..590e1410 100644 --- a/onboard/src/cv/config/crush.yaml +++ b/onboard/src/cv/config/crush.yaml @@ -1,6 +1,6 @@ usb_cameras: front: - device_path: /dev/video_front + device_path: /dev/video0 topic: /camera/usb/front/compressed horizontal_fov: 82 focal_length: 2.65 @@ -13,7 +13,7 @@ usb_cameras: frame_id: camera_usb_front bottom: - device_path: /dev/video_bottom + device_path: /dev/video4 topic: /camera/usb/bottom/compressed horizontal_fov: 82 focal_length: 2.65 diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 51a456b2..594c31a7 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -30,6 +30,8 @@ arduino: fqbn: arduino:avr:nano core: arduino:avr sketch: sketches/thruster + libraries: + - "'Adafruit BusIO'" gyro: ftdi: FT81SUWZ zero_bias: 0.0023800993 diff --git a/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.cpp b/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.cpp index fe4f7209..6ba2a447 100644 --- a/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.cpp +++ b/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.cpp @@ -1,196 +1,374 @@ -/*************************************************** - This is a library for our Adafruit 16-channel PWM & Servo driver - - This library was obtained from https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library +/*! + * @file Adafruit_PWMServoDriver.cpp + * + * @mainpage Adafruit 16-channel PWM & Servo driver + * + * @section intro_sec Introduction + * + * This is a library for the 16-channel PWM & Servo driver. + * + * Designed specifically to work with the Adafruit PWM & Servo driver. + * + * Pick one up today in the adafruit shop! + * ------> https://www.adafruit.com/product/815 + * + * These displays use I2C to communicate, 2 pins are required to interface. + * + * Adafruit invests time and resources providing this open source code, + * please support Adafruit andopen-source hardware by purchasing products + * from Adafruit! + * + * @section author Author + * + * Limor Fried/Ladyada (Adafruit Industries). + * + * @section license License + * + * BSD license, all text above must be included in any redistribution + */ - Pick one up today in the adafruit shop! - ------> http://www.adafruit.com/products/815 +#include "Adafruit_PWMServoDriver.h" - These displays use I2C to communicate, 2 pins are required to - interface. +//#define ENABLE_DEBUG_OUTPUT - Adafruit invests time and resources providing this open source code, - please support Adafruit and open-source hardware by purchasing - products from Adafruit! +/*! + * @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a + * TwoWire interface + */ +Adafruit_PWMServoDriver::Adafruit_PWMServoDriver() + : _i2caddr(PCA9685_I2C_ADDRESS), _i2c(&Wire) {} - Written by Limor Fried/Ladyada for Adafruit Industries. - BSD license, all text above must be included in any redistribution - ****************************************************/ +/*! + * @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a + * TwoWire interface + * @param addr The 7-bit I2C address to locate this chip, default is 0x40 + */ +Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(const uint8_t addr) + : _i2caddr(addr), _i2c(&Wire) {} -#include "Adafruit_PWMServoDriver.h" -#include +/*! + * @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a + * TwoWire interface + * @param addr The 7-bit I2C address to locate this chip, default is 0x40 + * @param i2c A reference to a 'TwoWire' object that we'll use to communicate + * with + */ +Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(const uint8_t addr, + TwoWire &i2c) + : _i2caddr(addr), _i2c(&i2c) {} -// Set to true to print some debug messages, or false to disable them. -//#define ENABLE_DEBUG_OUTPUT +/*! + * @brief Setups the I2C interface and hardware + * @param prescale + * Sets External Clock (Optional) + * @return true if successful, otherwise false + */ +bool Adafruit_PWMServoDriver::begin(uint8_t prescale) { + if (i2c_dev) + delete i2c_dev; + i2c_dev = new Adafruit_I2CDevice(_i2caddr, _i2c); + if (!i2c_dev->begin()) + return false; + reset(); + // set the default internal frequency + setOscillatorFrequency(FREQUENCY_OSCILLATOR); -/**************************************************************************/ -/*! - @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on the Wire interface. On Due we use Wire1 since its the interface on the 'default' I2C pins. - @param addr The 7-bit I2C address to locate this chip, default is 0x40 -*/ -/**************************************************************************/ -Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(uint8_t addr) { - _i2caddr = addr; + if (prescale) { + setExtClk(prescale); + } else { + // set a default frequency + setPWMFreq(1000); + } -#if defined(ARDUINO_SAM_DUE) - _i2c = &Wire1; -#else - _i2c = &Wire; -#endif + return true; } -/**************************************************************************/ /*! - @brief Instantiates a new PCA9685 PWM driver chip with the I2C address on a TwoWire interface - @param i2c A pointer to a 'Wire' compatible object that we'll use to communicate with - @param addr The 7-bit I2C address to locate this chip, default is 0x40 -*/ -/**************************************************************************/ -Adafruit_PWMServoDriver::Adafruit_PWMServoDriver(TwoWire *i2c, uint8_t addr) { - _i2c = i2c; - _i2caddr = addr; + * @brief Sends a reset command to the PCA9685 chip over I2C + */ +void Adafruit_PWMServoDriver::reset() { + write8(PCA9685_MODE1, MODE1_RESTART); + delay(10); } -/**************************************************************************/ /*! - @brief Setups the I2C interface and hardware -*/ -/**************************************************************************/ -void Adafruit_PWMServoDriver::begin(void) { - _i2c->begin(); - reset(); - // set a default frequency - setPWMFreq(1000); + * @brief Puts board into sleep mode + */ +void Adafruit_PWMServoDriver::sleep() { + uint8_t awake = read8(PCA9685_MODE1); + uint8_t sleep = awake | MODE1_SLEEP; // set sleep bit high + write8(PCA9685_MODE1, sleep); + delay(5); // wait until cycle ends for sleep to be active } +/*! + * @brief Wakes board from sleep + */ +void Adafruit_PWMServoDriver::wakeup() { + uint8_t sleep = read8(PCA9685_MODE1); + uint8_t wakeup = sleep & ~MODE1_SLEEP; // set sleep bit low + write8(PCA9685_MODE1, wakeup); +} -/**************************************************************************/ /*! - @brief Sends a reset command to the PCA9685 chip over I2C -*/ -/**************************************************************************/ -void Adafruit_PWMServoDriver::reset(void) { - write8(PCA9685_MODE1, 0x80); - delay(10); + * @brief Sets EXTCLK pin to use the external clock + * @param prescale + * Configures the prescale value to be used by the external clock + */ +void Adafruit_PWMServoDriver::setExtClk(uint8_t prescale) { + uint8_t oldmode = read8(PCA9685_MODE1); + uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep + write8(PCA9685_MODE1, newmode); // go to sleep, turn off internal oscillator + + // This sets both the SLEEP and EXTCLK bits of the MODE1 register to switch to + // use the external clock. + write8(PCA9685_MODE1, (newmode |= MODE1_EXTCLK)); + + write8(PCA9685_PRESCALE, prescale); // set the prescaler + + delay(5); + // clear the SLEEP bit to start + write8(PCA9685_MODE1, (newmode & ~MODE1_SLEEP) | MODE1_RESTART | MODE1_AI); + +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print("Mode now 0x"); + Serial.println(read8(PCA9685_MODE1), HEX); +#endif } -/**************************************************************************/ /*! - @brief Sets the PWM frequency for the entire chip, up to ~1.6 KHz - @param freq Floating point frequency that we will attempt to match -*/ -/**************************************************************************/ + * @brief Sets the PWM frequency for the entire chip, up to ~1.6 KHz + * @param freq Floating point frequency that we will attempt to match + */ void Adafruit_PWMServoDriver::setPWMFreq(float freq) { #ifdef ENABLE_DEBUG_OUTPUT Serial.print("Attempting to set freq "); Serial.println(freq); #endif + // Range output modulation frequency is dependant on oscillator + if (freq < 1) + freq = 1; + if (freq > 3500) + freq = 3500; // Datasheet limit is 3052=50MHz/(4*4096) - freq *= 0.9; // Correct for overshoot in the frequency setting (see issue #11). - float prescaleval = 25000000; - prescaleval /= 4096; - prescaleval /= freq; - prescaleval -= 1; + float prescaleval = ((_oscillator_freq / (freq * 4096.0)) + 0.5) - 1; + if (prescaleval < PCA9685_PRESCALE_MIN) + prescaleval = PCA9685_PRESCALE_MIN; + if (prescaleval > PCA9685_PRESCALE_MAX) + prescaleval = PCA9685_PRESCALE_MAX; + uint8_t prescale = (uint8_t)prescaleval; #ifdef ENABLE_DEBUG_OUTPUT - Serial.print("Estimated pre-scale: "); Serial.println(prescaleval); -#endif - - uint8_t prescale = floor(prescaleval + 0.5); -#ifdef ENABLE_DEBUG_OUTPUT - Serial.print("Final pre-scale: "); Serial.println(prescale); + Serial.print("Final pre-scale: "); + Serial.println(prescale); #endif uint8_t oldmode = read8(PCA9685_MODE1); - uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep - write8(PCA9685_MODE1, newmode); // go to sleep + uint8_t newmode = (oldmode & ~MODE1_RESTART) | MODE1_SLEEP; // sleep + write8(PCA9685_MODE1, newmode); // go to sleep write8(PCA9685_PRESCALE, prescale); // set the prescaler write8(PCA9685_MODE1, oldmode); delay(5); - write8(PCA9685_MODE1, oldmode | 0xa0); // This sets the MODE1 register to turn on auto increment. + // This sets the MODE1 register to turn on auto increment. + write8(PCA9685_MODE1, oldmode | MODE1_RESTART | MODE1_AI); #ifdef ENABLE_DEBUG_OUTPUT - Serial.print("Mode now 0x"); Serial.println(read8(PCA9685_MODE1), HEX); + Serial.print("Mode now 0x"); + Serial.println(read8(PCA9685_MODE1), HEX); #endif } -/**************************************************************************/ /*! - @brief Sets the PWM output of one of the PCA9685 pins - @param num One of the PWM output pins, from 0 to 15 - @param on At what point in the 4096-part cycle to turn the PWM output ON - @param off At what point in the 4096-part cycle to turn the PWM output OFF -*/ -/**************************************************************************/ -void Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, uint16_t off) { + * @brief Sets the output mode of the PCA9685 to either + * open drain or push pull / totempole. + * Warning: LEDs with integrated zener diodes should + * only be driven in open drain mode. + * @param totempole Totempole if true, open drain if false. + */ +void Adafruit_PWMServoDriver::setOutputMode(bool totempole) { + uint8_t oldmode = read8(PCA9685_MODE2); + uint8_t newmode; + if (totempole) { + newmode = oldmode | MODE2_OUTDRV; + } else { + newmode = oldmode & ~MODE2_OUTDRV; + } + write8(PCA9685_MODE2, newmode); +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print("Setting output mode: "); + Serial.print(totempole ? "totempole" : "open drain"); + Serial.print(" by setting MODE2 to "); + Serial.println(newmode); +#endif +} + +/*! + * @brief Reads set Prescale from PCA9685 + * @return prescale value + */ +uint8_t Adafruit_PWMServoDriver::readPrescale(void) { + return read8(PCA9685_PRESCALE); +} + +/*! + * @brief Gets the PWM output of one of the PCA9685 pins + * @param num One of the PWM output pins, from 0 to 15 + * @param off If true, returns PWM OFF value, otherwise PWM ON + * @return requested PWM output value + */ +uint16_t Adafruit_PWMServoDriver::getPWM(uint8_t num, bool off) { + uint8_t buffer[2] = {uint8_t(PCA9685_LED0_ON_L + 4 * num), 0}; + if (off) + buffer[0] += 2; + i2c_dev->write_then_read(buffer, 1, buffer, 2); + return uint16_t(buffer[0]) | (uint16_t(buffer[1]) << 8); +} + +/*! + * @brief Sets the PWM output of one of the PCA9685 pins + * @param num One of the PWM output pins, from 0 to 15 + * @param on At what point in the 4096-part cycle to turn the PWM output ON + * @param off At what point in the 4096-part cycle to turn the PWM output OFF + * @return 0 if successful, otherwise 1 + */ +uint8_t Adafruit_PWMServoDriver::setPWM(uint8_t num, uint16_t on, + uint16_t off) { #ifdef ENABLE_DEBUG_OUTPUT - Serial.print("Setting PWM "); Serial.print(num); Serial.print(": "); Serial.print(on); Serial.print("->"); Serial.println(off); + Serial.print("Setting PWM "); + Serial.print(num); + Serial.print(": "); + Serial.print(on); + Serial.print("->"); + Serial.println(off); #endif - _i2c->beginTransmission(_i2caddr); - _i2c->write(LED0_ON_L+4*num); - _i2c->write(on); - _i2c->write(on>>8); - _i2c->write(off); - _i2c->write(off>>8); - _i2c->endTransmission(); + uint8_t buffer[5]; + buffer[0] = PCA9685_LED0_ON_L + 4 * num; + buffer[1] = on; + buffer[2] = on >> 8; + buffer[3] = off; + buffer[4] = off >> 8; + + if (i2c_dev->write(buffer, 5)) { + return 0; + } else { + return 1; + } } -/**************************************************************************/ /*! - @brief Helper to set pin PWM output. Sets pin without having to deal with on/off tick placement and properly handles a zero value as completely off and 4095 as completely on. Optional invert parameter supports inverting the pulse for sinking to ground. - @param num One of the PWM output pins, from 0 to 15 - @param val The number of ticks out of 4096 to be active, should be a value from 0 to 4095 inclusive. - @param invert If true, inverts the output, defaults to 'false' -*/ -/**************************************************************************/ -void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert) -{ + * @brief Helper to set pin PWM output. Sets pin without having to deal with + * on/off tick placement and properly handles a zero value as completely off and + * 4095 as completely on. Optional invert parameter supports inverting the + * pulse for sinking to ground. + * @param num One of the PWM output pins, from 0 to 15 + * @param val The number of ticks out of 4096 to be active, should be a value + * from 0 to 4095 inclusive. + * @param invert If true, inverts the output, defaults to 'false' + */ +void Adafruit_PWMServoDriver::setPin(uint8_t num, uint16_t val, bool invert) { // Clamp value between 0 and 4095 inclusive. val = min(val, (uint16_t)4095); if (invert) { if (val == 0) { // Special value for signal fully on. setPWM(num, 4096, 0); - } - else if (val == 4095) { + } else if (val == 4095) { // Special value for signal fully off. setPWM(num, 0, 4096); + } else { + setPWM(num, 0, 4095 - val); } - else { - setPWM(num, 0, 4095-val); - } - } - else { + } else { if (val == 4095) { // Special value for signal fully on. setPWM(num, 4096, 0); - } - else if (val == 0) { + } else if (val == 0) { // Special value for signal fully off. setPWM(num, 0, 4096); - } - else { + } else { setPWM(num, 0, val); } } } -/*******************************************************************************************/ +/*! + * @brief Sets the PWM output of one of the PCA9685 pins based on the input + * microseconds, output is not precise + * @param num One of the PWM output pins, from 0 to 15 + * @param Microseconds The number of Microseconds to turn the PWM output ON + */ +void Adafruit_PWMServoDriver::writeMicroseconds(uint8_t num, + uint16_t Microseconds) { +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print("Setting PWM Via Microseconds on output"); + Serial.print(num); + Serial.print(": "); + Serial.print(Microseconds); + Serial.println("->"); +#endif -uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) { - _i2c->beginTransmission(_i2caddr); - _i2c->write(addr); - _i2c->endTransmission(); + double pulse = Microseconds; + double pulselength; + pulselength = 1000000; // 1,000,000 us per second + + // Read prescale + uint16_t prescale = readPrescale(); + +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print(prescale); + Serial.println(" PCA9685 chip prescale"); +#endif + + // Calculate the pulse for PWM based on Equation 1 from the datasheet section + // 7.3.5 + prescale += 1; + pulselength *= prescale; + pulselength /= _oscillator_freq; + +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print(pulselength); + Serial.println(" us per bit"); +#endif - _i2c->requestFrom((uint8_t)_i2caddr, (uint8_t)1); - return _i2c->read(); + pulse /= pulselength; + +#ifdef ENABLE_DEBUG_OUTPUT + Serial.print(pulse); + Serial.println(" pulse for PWM"); +#endif + + setPWM(num, 0, pulse); } -void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) { - _i2c->beginTransmission(_i2caddr); - _i2c->write(addr); - _i2c->write(d); - _i2c->endTransmission(); +/*! + * @brief Getter for the internally tracked oscillator used for freq + * calculations + * @returns The frequency the PCA9685 thinks it is running at (it cannot + * introspect) + */ +uint32_t Adafruit_PWMServoDriver::getOscillatorFrequency(void) { + return _oscillator_freq; } + +/*! + * @brief Setter for the internally tracked oscillator used for freq + * calculations + * @param freq The frequency the PCA9685 should use for frequency calculations + */ +void Adafruit_PWMServoDriver::setOscillatorFrequency(uint32_t freq) { + _oscillator_freq = freq; +} + +/******************* Low level I2C interface */ +uint8_t Adafruit_PWMServoDriver::read8(uint8_t addr) { + uint8_t buffer[1] = {addr}; + i2c_dev->write_then_read(buffer, 1, buffer, 1); + return buffer[0]; +} + +void Adafruit_PWMServoDriver::write8(uint8_t addr, uint8_t d) { + uint8_t buffer[2] = {addr, d}; + i2c_dev->write(buffer, 2); +} \ No newline at end of file diff --git a/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.h b/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.h index 85ec7518..9d5ab305 100644 --- a/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.h +++ b/onboard/src/offboard_comms/sketches/thruster/Adafruit_PWMServoDriver.h @@ -1,71 +1,106 @@ -/*************************************************** - This is a library for our Adafruit 16-channel PWM & Servo driver - - This library was obtained from https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library - - Pick one up today in the adafruit shop! - ------> http://www.adafruit.com/products/815 - - These displays use I2C to communicate, 2 pins are required to - interface. For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4 - - Adafruit invests time and resources providing this open source code, - please support Adafruit and open-source hardware by purchasing - products from Adafruit! - - Written by Limor Fried/Ladyada for Adafruit Industries. - BSD license, all text above must be included in any redistribution - ****************************************************/ - +/*! + * @file Adafruit_PWMServoDriver.h + * + * This is a library for our Adafruit 16-channel PWM & Servo driver. + * + * Designed specifically to work with the Adafruit 16-channel PWM & Servo + * driver. + * + * Pick one up today in the adafruit shop! + * ------> https://www.adafruit.com/product/815 + * + * These driver use I2C to communicate, 2 pins are required to interface. + * For Arduino UNOs, thats SCL -> Analog 5, SDA -> Analog 4. + * + * Adafruit invests time and resources providing this open source code, + * please support Adafruit andopen-source hardware by purchasing products + * from Adafruit! + * + * Limor Fried/Ladyada (Adafruit Industries). + * + * BSD license, all text above must be included in any redistribution + */ #ifndef _ADAFRUIT_PWMServoDriver_H #define _ADAFRUIT_PWMServoDriver_H -#if ARDUINO >= 100 - #include -#else - #include -#endif -#include +#include +#include + +// REGISTER ADDRESSES +#define PCA9685_MODE1 0x00 /**< Mode Register 1 */ +#define PCA9685_MODE2 0x01 /**< Mode Register 2 */ +#define PCA9685_SUBADR1 0x02 /**< I2C-bus subaddress 1 */ +#define PCA9685_SUBADR2 0x03 /**< I2C-bus subaddress 2 */ +#define PCA9685_SUBADR3 0x04 /**< I2C-bus subaddress 3 */ +#define PCA9685_ALLCALLADR 0x05 /**< LED All Call I2C-bus address */ +#define PCA9685_LED0_ON_L 0x06 /**< LED0 on tick, low byte*/ +#define PCA9685_LED0_ON_H 0x07 /**< LED0 on tick, high byte*/ +#define PCA9685_LED0_OFF_L 0x08 /**< LED0 off tick, low byte */ +#define PCA9685_LED0_OFF_H 0x09 /**< LED0 off tick, high byte */ +// etc all 16: LED15_OFF_H 0x45 +#define PCA9685_ALLLED_ON_L 0xFA /**< load all the LEDn_ON registers, low */ +#define PCA9685_ALLLED_ON_H 0xFB /**< load all the LEDn_ON registers, high */ +#define PCA9685_ALLLED_OFF_L 0xFC /**< load all the LEDn_OFF registers, low */ +#define PCA9685_ALLLED_OFF_H 0xFD /**< load all the LEDn_OFF registers,high */ +#define PCA9685_PRESCALE 0xFE /**< Prescaler for PWM output frequency */ +#define PCA9685_TESTMODE 0xFF /**< defines the test mode to be entered */ + +// MODE1 bits +#define MODE1_ALLCAL 0x01 /**< respond to LED All Call I2C-bus address */ +#define MODE1_SUB3 0x02 /**< respond to I2C-bus subaddress 3 */ +#define MODE1_SUB2 0x04 /**< respond to I2C-bus subaddress 2 */ +#define MODE1_SUB1 0x08 /**< respond to I2C-bus subaddress 1 */ +#define MODE1_SLEEP 0x10 /**< Low power mode. Oscillator off */ +#define MODE1_AI 0x20 /**< Auto-Increment enabled */ +#define MODE1_EXTCLK 0x40 /**< Use EXTCLK pin clock */ +#define MODE1_RESTART 0x80 /**< Restart enabled */ +// MODE2 bits +#define MODE2_OUTNE_0 0x01 /**< Active LOW output enable input */ +#define MODE2_OUTNE_1 \ + 0x02 /**< Active LOW output enable input - high impedience */ +#define MODE2_OUTDRV 0x04 /**< totem pole structure vs open-drain */ +#define MODE2_OCH 0x08 /**< Outputs change on ACK vs STOP */ +#define MODE2_INVRT 0x10 /**< Output logic state inverted */ + +#define PCA9685_I2C_ADDRESS 0x40 /**< Default PCA9685 I2C Slave Address */ +#define FREQUENCY_OSCILLATOR 25000000 /**< Int. osc. frequency in datasheet */ + +#define PCA9685_PRESCALE_MIN 3 /**< minimum prescale value */ +#define PCA9685_PRESCALE_MAX 255 /**< maximum prescale value */ -#define PCA9685_SUBADR1 0x2 -#define PCA9685_SUBADR2 0x3 -#define PCA9685_SUBADR3 0x4 - -#define PCA9685_MODE1 0x0 -#define PCA9685_PRESCALE 0xFE - -#define LED0_ON_L 0x6 -#define LED0_ON_H 0x7 -#define LED0_OFF_L 0x8 -#define LED0_OFF_H 0x9 - -#define ALLLED_ON_L 0xFA -#define ALLLED_ON_H 0xFB -#define ALLLED_OFF_L 0xFC -#define ALLLED_OFF_H 0xFD - -/**************************************************************************/ /*! - @brief Class that stores state and functions for interacting with PCA9685 PWM chip -*/ -/**************************************************************************/ + * @brief Class that stores state and functions for interacting with PCA9685 + * PWM chip + */ class Adafruit_PWMServoDriver { - public: - Adafruit_PWMServoDriver(uint8_t addr = 0x40); - Adafruit_PWMServoDriver(TwoWire *I2C, uint8_t addr = 0x40); - void begin(void); - void reset(void); +public: + Adafruit_PWMServoDriver(); + Adafruit_PWMServoDriver(const uint8_t addr); + Adafruit_PWMServoDriver(const uint8_t addr, TwoWire &i2c); + bool begin(uint8_t prescale = 0); + void reset(); + void sleep(); + void wakeup(); + void setExtClk(uint8_t prescale); void setPWMFreq(float freq); - void setPWM(uint8_t num, uint16_t on, uint16_t off); - void setPin(uint8_t num, uint16_t val, bool invert=false); + void setOutputMode(bool totempole); + uint16_t getPWM(uint8_t num, bool off = false); + uint8_t setPWM(uint8_t num, uint16_t on, uint16_t off); + void setPin(uint8_t num, uint16_t val, bool invert = false); + uint8_t readPrescale(void); + void writeMicroseconds(uint8_t num, uint16_t Microseconds); - private: - uint8_t _i2caddr; + void setOscillatorFrequency(uint32_t freq); + uint32_t getOscillatorFrequency(void); +private: + uint8_t _i2caddr; TwoWire *_i2c; + Adafruit_I2CDevice *i2c_dev = NULL; ///< Pointer to I2C bus interface + uint32_t _oscillator_freq; uint8_t read8(uint8_t addr); void write8(uint8_t addr, uint8_t d); }; -#endif +#endif \ No newline at end of file diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index ab038b87..0aa8ad31 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -6,6 +6,9 @@ #define OOGWAY_SHELL 1 #define CRUSH 2 +#define CRUSH_MUX_OSCILLATOR_FREQ 27800000 +#define OOGWAY_MUX_OSCILLATOR_FREQ 27800000 // todo + #define BAUD_RATE 57600 #define THRUSTER_TIMEOUT_MS 1000 #define THRUSTER_STOP_PWM 1500 @@ -56,7 +59,7 @@ void setup() { break; case CRUSH: NUM_THRUSTERS = 8; - THRUSTER_PWM_OFFSET = 63; + THRUSTER_PWM_OFFSET = 203; break; default: valid_robot = false; @@ -67,6 +70,13 @@ void setup() { thrusters = new MultiplexedBasicESC[NUM_THRUSTERS]; pwm_multiplexer.begin(); + + if (ROBOT_NAME == CRUSH) { + pwm_multiplexer.setOscillatorFrequency(CRUSH_MUX_OSCILLATOR_FREQ); + } else { + pwm_multiplexer.setOscillatorFrequency(OOGWAY_MUX_OSCILLATOR_FREQ); + } + for (uint8_t i = 0; i < NUM_THRUSTERS; i++) { thrusters[i].initialize(&pwm_multiplexer); thrusters[i].attach(i); From c9658adee5b6c313fc31fa227c9ee7b0a8fa0a96 Mon Sep 17 00:00:00 2001 From: Crush Date: Fri, 13 Feb 2026 12:34:30 -0800 Subject: [PATCH 08/19] updated configs for new mux --- onboard/src/offboard_comms/sketches/thruster/thruster.ino | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index 0aa8ad31..bcceb79b 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -6,7 +6,7 @@ #define OOGWAY_SHELL 1 #define CRUSH 2 -#define CRUSH_MUX_OSCILLATOR_FREQ 27800000 +#define CRUSH_MUX_OSCILLATOR_FREQ 27000000 #define OOGWAY_MUX_OSCILLATOR_FREQ 27800000 // todo #define BAUD_RATE 57600 @@ -59,7 +59,7 @@ void setup() { break; case CRUSH: NUM_THRUSTERS = 8; - THRUSTER_PWM_OFFSET = 203; + THRUSTER_PWM_OFFSET = 175; break; default: valid_robot = false; From 1d2f08fbdebdaca43bff3df3e43b68f1b544417e Mon Sep 17 00:00:00 2001 From: Crush Date: Fri, 13 Feb 2026 23:02:31 +0000 Subject: [PATCH 09/19] tuned thruster pwn offset and the oscillator frequency git push ; --- onboard/src/offboard_comms/config/crush.yaml | 6 +++--- onboard/src/offboard_comms/sketches/thruster/thruster.ino | 4 ++-- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 594c31a7..4fa9ef57 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -40,9 +40,9 @@ gyro: modem: ftdi: CP2102 thrusters: - - name: middle_right_first # TODO: Validate cw and standard_esc values for all middle_left and middle_right thrusters + - name: middle_right_first cw: false - standard_esc: false + standard_esc: true - name: middle_right_second cw: false standard_esc: false @@ -63,4 +63,4 @@ thrusters: standard_esc: true - name: front_left cw: true - standard_esc: false + standard_esc: true diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index bcceb79b..b40f4746 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -6,7 +6,7 @@ #define OOGWAY_SHELL 1 #define CRUSH 2 -#define CRUSH_MUX_OSCILLATOR_FREQ 27000000 +#define CRUSH_MUX_OSCILLATOR_FREQ 28000000 #define OOGWAY_MUX_OSCILLATOR_FREQ 27800000 // todo #define BAUD_RATE 57600 @@ -59,7 +59,7 @@ void setup() { break; case CRUSH: NUM_THRUSTERS = 8; - THRUSTER_PWM_OFFSET = 175; + THRUSTER_PWM_OFFSET = 210; break; default: valid_robot = false; From 795dc630afd1a8eaae71985470560dd068a8a867 Mon Sep 17 00:00:00 2001 From: Crush Date: Thu, 26 Mar 2026 23:41:40 +0000 Subject: [PATCH 10/19] crush yaml and thruster stuff --- onboard/src/offboard_comms/config/crush.yaml | 2 +- onboard/src/offboard_comms/sketches/thruster/thruster.ino | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 4fa9ef57..2aa8925b 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -26,7 +26,7 @@ arduino: tag: HB topic: /sensors/humidity/battery thruster: - ftdi: A10LT2EI + ftdi: AQ02V0UK fqbn: arduino:avr:nano core: arduino:avr sketch: sketches/thruster diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index b40f4746..87c8533d 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -6,7 +6,8 @@ #define OOGWAY_SHELL 1 #define CRUSH 2 -#define CRUSH_MUX_OSCILLATOR_FREQ 28000000 +// #define CRUSH_MUX_OSCILLATOR_FREQ 26150000 +#define CRUSH_MUX_OSCILLATOR_FREQ 25100000 #define OOGWAY_MUX_OSCILLATOR_FREQ 27800000 // todo #define BAUD_RATE 57600 From 63ffd35f52a33f6017643c73938880c969b29bda Mon Sep 17 00:00:00 2001 From: Crush Date: Sat, 4 Apr 2026 12:52:55 -0700 Subject: [PATCH 11/19] tuned positions of thrusters --- onboard/src/offboard_comms/config/crush.yaml | 8 ++++---- .../task_planning/task_planning/robot/crush.py | 18 +++++++++--------- .../task_planning/robot/oogway.py | 4 ++-- .../task_planning/task_planning/task_runner.py | 4 ++-- 4 files changed, 17 insertions(+), 17 deletions(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 2aa8925b..87b9f319 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -34,21 +34,21 @@ arduino: - "'Adafruit BusIO'" gyro: ftdi: FT81SUWZ - zero_bias: 0.0023800993 + zero_bias: 0.0024940463 scale_factor: 15497811.0 negate: true modem: ftdi: CP2102 thrusters: + - name: front_right + cw: false + standard_esc: false - name: middle_right_first cw: false standard_esc: true - name: middle_right_second cw: false standard_esc: false - - name: front_right - cw: false - standard_esc: false - name: back_right cw: false 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 efbe15fa..e4a7e0a4 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -3,9 +3,9 @@ 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, ivc_tasks, move_tasks, prequal_tasks, sonar_tasks +from task_planning.tasks import buoyancy_tasks, comp_tasks, move_tasks, prequal_tasks, sonar_tasks from task_planning.utils import geometry_utils @@ -14,15 +14,15 @@ 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), + # 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 + # 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.crush_ivc_spam(msg_to_send=IVCMessageType.CRUSH_OCTAGON, parent=self), + # 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), ######## Unused competition tasks ######## ## Gate diff --git a/onboard/src/task_planning/task_planning/robot/oogway.py b/onboard/src/task_planning/task_planning/robot/oogway.py index cbaff9ae..753e35ca 100644 --- a/onboard/src/task_planning/task_planning/robot/oogway.py +++ b/onboard/src/task_planning/task_planning/robot/oogway.py @@ -2,13 +2,13 @@ from math import radians from task_planning.interface.cv import CVObjectType -from task_planning.interface.ivc import IVCMessageType +# from task_planning.interface.ivc import IVCMessageType from task_planning.interface.servos import TorpedoStates from task_planning.task import Task, task from task_planning.tasks import ( buoyancy_tasks, comp_tasks, - ivc_tasks, + # ivc_tasks, move_tasks, prequal_tasks, servos_tasks, diff --git a/onboard/src/task_planning/task_planning/task_runner.py b/onboard/src/task_planning/task_planning/task_runner.py index d7cd5abb..74966117 100644 --- a/onboard/src/task_planning/task_planning/task_runner.py +++ b/onboard/src/task_planning/task_planning/task_runner.py @@ -9,7 +9,7 @@ from task_planning.interface.controls import Controls from task_planning.interface.cv import CV -from task_planning.interface.ivc import IVC +# from task_planning.interface.ivc import IVC from task_planning.interface.servos import Servos from task_planning.interface.sonar import Sonar from task_planning.interface.state import State @@ -45,7 +45,7 @@ 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) State(self, tf_buffer=tf_buffer, bypass=self.bypass) From 3164feca76a249d4269e3e662122fb79d27f3810 Mon Sep 17 00:00:00 2001 From: Crush Date: Thu, 1 Jan 1970 00:00:35 -0028 Subject: [PATCH 12/19] changed lists to match wiring + recomputed wrench --- onboard/src/controls/config/crush.yaml | 8 ++++---- onboard/src/controls/data/crush_wrench.csv | 12 ++++++------ onboard/src/controls/data/crush_wrench_pinv.csv | 16 ++++++++-------- onboard/src/offboard_comms/config/crush.yaml | 4 ++-- 4 files changed, 20 insertions(+), 20 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index f5804819..c5b020a5 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -208,6 +208,10 @@ static_power_global: power_scale_factor: 1 # all x positions assume the corner link is 10.41527MM +x than it actually is thrusters: + - name: front_right + type: T200 + pos: [0.001895, -0.38866, -0.003175] + rpy: [0, 0, -135] - name: middle_right_first type: T200 pos: [-0.15501, -0.34805, -0.003175] @@ -216,10 +220,6 @@ thrusters: type: T200 pos: [-0.32582, -0.34805, -0.003175] rpy: [0, 90, 0] - - name: front_right - type: T200 - pos: [0.001895, -0.38866, -0.003175] - rpy: [0, 0, -135] - name: back_right type: T200 pos: [-0.48273, -0.38866, -0.003175] diff --git a/onboard/src/controls/data/crush_wrench.csv b/onboard/src/controls/data/crush_wrench.csv index 11200e2f..3210bd0a 100644 --- a/onboard/src/controls/data/crush_wrench.csv +++ b/onboard/src/controls/data/crush_wrench.csv @@ -1,6 +1,6 @@ -0.0,0.0,-0.7071067812,-0.7071067812,-0.7071067812,0.0,0.0,-0.7071067812 -0.0,0.0,-0.7071067812,0.7071067812,-0.7071067812,0.0,0.0,0.7071067812 --1.0,-1.0,0.0,0.0,0.0,-1.0,-1.0,0.0 -0.6576125,0.6576125,0.1234785217,-0.1234785217,0.1234785217,0.2542125,0.2542125,-0.1234785217 -0.0754315,-0.0953785,-0.1234785217,-0.1234785217,-0.1234785217,0.0754315,-0.0953785,-0.1234785217 -0.0,0.0,-0.6580045792,-0.6721127737,0.0273551329,0.0,0.0,0.0132469384 +-0.7071067812,0.0,0.0,-0.7071067812,-0.7071067812,0.0,0.0,-0.7071067812 +-0.7071067812,0.0,0.0,0.7071067812,-0.7071067812,0.0,0.0,0.7071067812 +0.0,-1.0,-1.0,0.0,0.0,-1.0,-1.0,0.0 +0.0104651804,0.2017,0.2017,-0.0104651804,0.0104651804,-0.2017,-0.2017,-0.0104651804 +-0.0104651804,0.0854052701,-0.0854047299,-0.0104651804,-0.0104651804,0.0854052701,-0.0854047299,-0.0104651804 +-0.3426782793,0.0,0.0,-0.3426814328,0.3426814328,0.0,0.0,0.3426782793 \ No newline at end of file diff --git a/onboard/src/controls/data/crush_wrench_pinv.csv b/onboard/src/controls/data/crush_wrench_pinv.csv index 01800689..337d5b09 100644 --- a/onboard/src/controls/data/crush_wrench_pinv.csv +++ b/onboard/src/controls/data/crush_wrench_pinv.csv @@ -1,8 +1,8 @@ --0.511167379,0.2164414973,0.285892663,1.2394645513,2.9272290849,0.0 -0.511167379,0.2164414973,0.3442821015,1.2394645513,-2.9272290849,0.0 --0.0209452055,-0.3608313206,0.0,0.0,0.0,-0.729543904 --0.0209452055,0.3462754606,0.0,0.0,0.0,-0.729543904 --0.6861615757,-0.3462754606,0.0,0.0,0.0,0.729543904 --0.511167379,-0.2164414973,-0.8442821015,-1.2394645513,2.9272290849,0.0 -0.511167379,-0.2164414973,-0.785892663,-1.2394645513,-2.9272290849,0.0 --0.6861615757,0.3608313206,0.0,0.0,0.0,0.729543904 +-0.3535533906,-0.3535550174,0.0,0.0,0.0,-0.729543904 +-0.0433229905,0.0183440754,-0.2499992093,1.2394645513,2.9272290849,0.0 +0.0433229905,0.0183440754,-0.2500007907,1.2394645513,-2.9272290849,0.0 +-0.3535533906,0.3535517638,0.0,0.0,0.0,-0.729543904 +-0.3535533906,-0.3535517638,0.0,0.0,0.0,0.729543904 +-0.0433229905,-0.0183440754,-0.2499992093,-1.2394645513,2.9272290849,0.0 +0.0433229905,-0.0183440754,-0.2500007907,-1.2394645513,-2.9272290849,0.0 +-0.3535533906,0.3535550174,0.0,0.0,0.0,0.729543904 \ 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 87b9f319..7418a294 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -44,7 +44,7 @@ thrusters: cw: false standard_esc: false - name: middle_right_first - cw: false + cw: true standard_esc: true - name: middle_right_second cw: false @@ -59,7 +59,7 @@ thrusters: cw: true standard_esc: true - name: middle_left_second - cw: true + cw: false standard_esc: true - name: front_left cw: true From a8209dd600c99d26ce96bdc0c369b1baf6cd6abe Mon Sep 17 00:00:00 2001 From: Crush Date: Thu, 1 Jan 1970 00:00:51 -0045 Subject: [PATCH 13/19] tuned crush configs --- onboard/src/offboard_comms/config/crush.yaml | 10 +++++----- onboard/src/task_planning/task_planning/robot/crush.py | 3 ++- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/onboard/src/offboard_comms/config/crush.yaml b/onboard/src/offboard_comms/config/crush.yaml index 7418a294..325f492b 100644 --- a/onboard/src/offboard_comms/config/crush.yaml +++ b/onboard/src/offboard_comms/config/crush.yaml @@ -44,22 +44,22 @@ thrusters: cw: false standard_esc: false - name: middle_right_first - cw: true + cw: false standard_esc: true - name: middle_right_second - cw: false + cw: true standard_esc: false - name: back_right cw: false standard_esc: true - name: back_left cw: true - standard_esc: false + standard_esc: true - name: middle_left_first - cw: true + cw: false standard_esc: true - name: middle_left_second - cw: false + cw: true standard_esc: true - name: front_left cw: true diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index e4a7e0a4..60d5a470 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -15,7 +15,8 @@ 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, z_tolerance=0.15, enable_controls_flag=True, timeout=10, 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(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 From 4b1718e779c6ab31a152d468ae1ffd6ad14d8a2d Mon Sep 17 00:00:00 2001 From: Crush Date: Sat, 11 Apr 2026 10:07:01 -0700 Subject: [PATCH 14/19] remove hsv filtering --- .../src/cv/launch/usb_camera_detectors.launch.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/onboard/src/cv/launch/usb_camera_detectors.launch.py b/onboard/src/cv/launch/usb_camera_detectors.launch.py index bcb7ab93..6dd52533 100644 --- a/onboard/src/cv/launch/usb_camera_detectors.launch.py +++ b/onboard/src/cv/launch/usb_camera_detectors.launch.py @@ -34,12 +34,12 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(IncludeLaunchDescription( XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'torpedo_target_detector.xml')), )) - elif robot_name in ['crush']: - ld.add_action(IncludeLaunchDescription( - XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_front.xml')), - )) - ld.add_action(IncludeLaunchDescription( - XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_bottom.xml')), - )) + # elif robot_name in ['crush']: + # ld.add_action(IncludeLaunchDescription( + # XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_front.xml')), + # )) + # ld.add_action(IncludeLaunchDescription( + # XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_bottom.xml')), + # )) return ld From a671e9db9696f8d03c7c4cddc38eb03dd33ece08 Mon Sep 17 00:00:00 2001 From: Crush Date: Sun, 12 Apr 2026 09:10:21 -0700 Subject: [PATCH 15/19] fixed buoyancy task --- .../src/task_planning/task_planning/tasks/buoyancy_tasks.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/onboard/src/task_planning/task_planning/tasks/buoyancy_tasks.py b/onboard/src/task_planning/task_planning/tasks/buoyancy_tasks.py index ca3a8353..e64229ee 100644 --- a/onboard/src/task_planning/task_planning/tasks/buoyancy_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/buoyancy_tasks.py @@ -18,9 +18,9 @@ async def tune_static_power(self: Task) -> Task[None, None, None]: DELTA_SCALE_FACTOR = 0.65 MAX_ERROR = 0.05 curr_depth = State().depth - TARGET_DEPTH = State().orig_depth - 1.0 + TARGET_DEPTH = State().orig_depth - 0.5 delta = curr_depth - TARGET_DEPTH - move_to_depth = -1.0 + move_to_depth = TARGET_DEPTH while (abs(delta) > MAX_ERROR): logger.info(f'Move to depth: {move_to_depth}') From aa581c31bb162d149c0a5448bc452982f0ca598e Mon Sep 17 00:00:00 2001 From: Crush Date: Sun, 12 Apr 2026 09:12:21 -0700 Subject: [PATCH 16/19] tuned crush x y z yaw PID --- onboard/src/controls/config/crush.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index c5b020a5..daa3399e 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -52,8 +52,8 @@ pid: error_ramp_rate: 1000 yaw: Kp: 0.09 - Ki: 0.003 - Kd: 0 + Ki: 0 + Kd: 0.02 Ff: 0 control_effort: min: -1 From 95fbc5fd6d471bbc48d22eda6d9fe260aaf3f877 Mon Sep 17 00:00:00 2001 From: Crush Date: Sun, 12 Apr 2026 13:36:55 -0700 Subject: [PATCH 17/19] coin flip works, barrel roll sleep time needs tuning, waiting for mechanical leak fix --- .../task_planning/robot/crush.py | 4 +-- .../task_planning/tasks/comp_tasks.py | 28 +++++++++---------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/onboard/src/task_planning/task_planning/robot/crush.py b/onboard/src/task_planning/task_planning/robot/crush.py index 60d5a470..c37d569a 100644 --- a/onboard/src/task_planning/task_planning/robot/crush.py +++ b/onboard/src/task_planning/task_planning/robot/crush.py @@ -17,9 +17,9 @@ async def main(self: Task) -> Task[None, None, None]: # ivc_tasks.delineate_ivc_log(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(enable_same_direction=False, 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.975, 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), diff --git a/onboard/src/task_planning/task_planning/tasks/comp_tasks.py b/onboard/src/task_planning/task_planning/tasks/comp_tasks.py index 7de9ebf0..e731e840 100644 --- a/onboard/src/task_planning/task_planning/tasks/comp_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/comp_tasks.py @@ -107,18 +107,18 @@ async def coin_flip(self: CompTask, depth_level: float = 0.7, if yaw_correction > np.pi: yaw_correction -= np.pi - await self.correct_yaw(np.pi, timeout=timeout) + await self.correct_yaw(np.pi, yaw_tolerance=0.1, timeout=timeout) logger.info('[coin_flip] Yaw correct 180') logger.info(f'[coin_flip] Yaw correct remainder: {yaw_correction}') - await self.correct_yaw(yaw_correction, timeout=timeout) + await self.correct_yaw(yaw_correction, yaw_tolerance=0.1, timeout=timeout) else: while abs(State().get_gyro_yaw_correction(return_raw=True)) > math.radians(5): yaw_correction = State().get_gyro_yaw_correction(return_raw=False, maximum_yaw=2*np.pi) logger.info(f'[coin_flip] Yaw correction: {yaw_correction}') - await self.correct_yaw(yaw_correction, timeout=timeout) + await self.correct_yaw(yaw_correction, yaw_tolerance=0.1, timeout=timeout) logger.info(f'[coin_flip] Final yaw offset: {State().get_gyro_yaw_correction(return_raw=True)}') @@ -188,7 +188,7 @@ async def roll() -> None: if get_robot_name() == RobotName.OOGWAY: await util_tasks.sleep(2.25, parent=self) else: - await util_tasks.sleep(1.40, parent=self) + await util_tasks.sleep(3, parent=self) logger.info('[gate_style_task] Completed roll') @@ -200,19 +200,19 @@ async def roll() -> None: await self.correct_depth(DEPTH_LEVEL) await roll() - State().reset_pose() - await util_tasks.sleep(2.5, parent=self) + # State().reset_pose() + await util_tasks.sleep(1.8, parent=self) - await self.correct_depth(DEPTH_LEVEL) - await roll() - State().reset_pose() - await util_tasks.sleep(2.5, parent=self) + # await self.correct_depth(DEPTH_LEVEL) + # await roll() + # State().reset_pose() + # await util_tasks.sleep(2.5, parent=self) - await self.correct_depth(DEPTH_LEVEL) - await util_tasks.sleep(2.5, parent=self) + # await self.correct_depth(DEPTH_LEVEL) + # await util_tasks.sleep(2.5, parent=self) - await self.correct_roll_and_pitch() - logger.info('[gate_style_task] Reset orientation') + # await self.correct_roll_and_pitch() + # logger.info('[gate_style_task] Reset orientation') @comp_task From 26777e88f345dabee1ff6d5df9fd4af091660fdf Mon Sep 17 00:00:00 2001 From: Patrick Zheng Date: Thu, 16 Apr 2026 14:04:24 -0400 Subject: [PATCH 18/19] added tech debt comment for reference --- onboard/src/controls/config/crush.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/onboard/src/controls/config/crush.yaml b/onboard/src/controls/config/crush.yaml index daa3399e..57f83609 100644 --- a/onboard/src/controls/config/crush.yaml +++ b/onboard/src/controls/config/crush.yaml @@ -207,6 +207,7 @@ static_power_global: 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 From cd522689db67000fb2ae3b884f5d3fa36531b5e4 Mon Sep 17 00:00:00 2001 From: Patrick Zheng Date: Thu, 16 Apr 2026 14:11:30 -0400 Subject: [PATCH 19/19] revert unrelated changes --- onboard/src/cv/config/crush.yaml | 4 ++-- .../src/cv/launch/usb_camera_detectors.launch.py | 14 +++++++------- .../offboard_comms/sketches/thruster/thruster.ino | 7 ++++--- .../task_planning/tasks/prequal_tasks.py | 1 - 4 files changed, 13 insertions(+), 13 deletions(-) diff --git a/onboard/src/cv/config/crush.yaml b/onboard/src/cv/config/crush.yaml index 590e1410..0c23836a 100644 --- a/onboard/src/cv/config/crush.yaml +++ b/onboard/src/cv/config/crush.yaml @@ -1,6 +1,6 @@ usb_cameras: front: - device_path: /dev/video0 + device_path: /dev/video_front topic: /camera/usb/front/compressed horizontal_fov: 82 focal_length: 2.65 @@ -13,7 +13,7 @@ usb_cameras: frame_id: camera_usb_front bottom: - device_path: /dev/video4 + device_path: /dev/video_bottom topic: /camera/usb/bottom/compressed horizontal_fov: 82 focal_length: 2.65 diff --git a/onboard/src/cv/launch/usb_camera_detectors.launch.py b/onboard/src/cv/launch/usb_camera_detectors.launch.py index 6dd52533..bcb7ab93 100644 --- a/onboard/src/cv/launch/usb_camera_detectors.launch.py +++ b/onboard/src/cv/launch/usb_camera_detectors.launch.py @@ -34,12 +34,12 @@ def generate_launch_description() -> LaunchDescription: ld.add_action(IncludeLaunchDescription( XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'torpedo_target_detector.xml')), )) - # elif robot_name in ['crush']: - # ld.add_action(IncludeLaunchDescription( - # XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_front.xml')), - # )) - # ld.add_action(IncludeLaunchDescription( - # XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_bottom.xml')), - # )) + elif robot_name in ['crush']: + ld.add_action(IncludeLaunchDescription( + XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_front.xml')), + )) + ld.add_action(IncludeLaunchDescription( + XMLLaunchDescriptionSource(str(pkg_cv / 'launch' / 'hsv_pink_bin_bottom.xml')), + )) return ld diff --git a/onboard/src/offboard_comms/sketches/thruster/thruster.ino b/onboard/src/offboard_comms/sketches/thruster/thruster.ino index 87c8533d..9d430023 100644 --- a/onboard/src/offboard_comms/sketches/thruster/thruster.ino +++ b/onboard/src/offboard_comms/sketches/thruster/thruster.ino @@ -8,7 +8,7 @@ // #define CRUSH_MUX_OSCILLATOR_FREQ 26150000 #define CRUSH_MUX_OSCILLATOR_FREQ 25100000 -#define OOGWAY_MUX_OSCILLATOR_FREQ 27800000 // todo +#define OOGWAY_MUX_OSCILLATOR_FREQ 0 // TODO: Tune Oogway Mux #define BAUD_RATE 57600 #define THRUSTER_TIMEOUT_MS 1000 @@ -74,9 +74,10 @@ void setup() { if (ROBOT_NAME == CRUSH) { pwm_multiplexer.setOscillatorFrequency(CRUSH_MUX_OSCILLATOR_FREQ); - } else { - pwm_multiplexer.setOscillatorFrequency(OOGWAY_MUX_OSCILLATOR_FREQ); } + // else { + // pwm_multiplexer.setOscillatorFrequency(OOGWAY_MUX_OSCILLATOR_FREQ); + // } for (uint8_t i = 0; i < NUM_THRUSTERS; i++) { thrusters[i].initialize(&pwm_multiplexer); diff --git a/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py b/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py index 9f7014fa..b75c5d9c 100644 --- a/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py +++ b/onboard/src/task_planning/task_planning/tasks/prequal_tasks.py @@ -16,7 +16,6 @@ LANE_MARKER_HEIGHT_METERS = 0.3048 - @task async def prequal_task(self: Task) -> Task[None, None, None]: # noqa: PLR0915 """Complete the prequalification task by tracking the lane marker."""