From b5e573783ba1e7aa6eacea82a873869ca02b9300 Mon Sep 17 00:00:00 2001 From: Wang Mak Date: Sat, 12 Apr 2025 16:36:09 -0400 Subject: [PATCH 1/3] Trim Sail & Control Mode --- .../sailboat_main/teensy/teensy.py | 10 ++- .../sailboat_main/teensy/teensy_node.py | 26 +++++++- teensy/src/Control Tasks/ServoControlTask.cpp | 64 +++++++++++++------ teensy/src/Monitors/SerialMonitor.cpp | 1 + teensy/src/sfr.cpp | 6 +- teensy/src/sfr.hpp | 3 +- 6 files changed, 80 insertions(+), 30 deletions(-) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy.py b/src/sailboat_main/sailboat_main/teensy/teensy.py index 577b01d..d7554ce 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy.py @@ -75,12 +75,13 @@ def _parse_packet(self, packet): return wind_angle, sail_angle, rudder_angle, dropped_packets - def send_command(self, sail, rudder): + def send_command(self, sail, rudder, mode): """ Send a properly formatted command packet to the servo. :param sail: Sail position (integer) :param tail: Tail position (integer) + :param mode: Control mode (boolean) """ try: # check bounds @@ -91,8 +92,11 @@ def send_command(self, sail, rudder): sail_byte = sail & 0xFF if sail >= 0 else (sail + 256) & 0xFF rudder_byte = rudder & 0xFF if rudder >= 0 else (rudder + 256) & 0xFF - # create the packet: [start flag] [sail] [tail] [end flag] - command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, self.END_BYTE]) + # convert mode to byte (0 for RC, 1 for autonomous) + mode_byte = 1 if mode else 0 + + # create the packet: [start flag] [sail] [tail] [mode] [end flag] + command_packet = bytearray([self.START_BYTE, sail_byte, rudder_byte, mode_byte, self.END_BYTE]) # send the packet over serial self.serial.write(command_packet) diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 91f8021..9b41393 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_node.py @@ -2,6 +2,7 @@ from rclpy.node import Node from sailboat_interface.msg import SailTail from std_msgs.msg import Int32 +from std_msgs.msg import String from . import teensy from . import teensy_fake @@ -45,6 +46,9 @@ def __init__(self): self.desired_sail = 0 self.desired_rudder = 0 + # keep track of the control mode, default to RC mode + self.autonomous_mode = False + # telemetry data publishers self.wind_angle_pub = self.create_publisher(Int32, 'wind', 10) self.actual_sail_angle_pub = self.create_publisher(Int32, 'actual_sail_angle', 10) @@ -66,6 +70,13 @@ def __init__(self): 'rudder_angle', self.rudder_callback, 10) + + # subscription to control mode topic + self.subscription = self.create_subscription( + String, + 'control_mode', + self.control_mode_callback, + 10) def check_telemetry(self): """ @@ -106,7 +117,7 @@ def sail_callback(self, msg): self.desired_sail = msg.data #self.get_logger().info(f"Sail callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.autonomous_mode) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") @@ -119,11 +130,22 @@ def rudder_callback(self, msg): self.desired_rudder = msg.data self.get_logger().info(f"Rudder callback-sent to Teensy sail:{self.desired_sail}, rudder: {self.desired_rudder}") - if self.teensy.send_command(self.desired_sail, self.desired_rudder) == 0: + if self.teensy.send_command(self.desired_sail, self.desired_rudder, self.autonomous_mode) == 0: self.get_logger().info(f"Message sent to servo") else: self.get_logger().warn(f"Message failed to send to servo") + def control_mode_callback(self, msg): + """ + Callback function for the 'control_mode' topic. Updates the control mode + based on the received message. + """ + if msg.data == 'radio': + self.autonomous_mode = False # RC Mode + elif msg.data == 'algorithm': + self.autonomous_mode = True # Autonomous Mode + self.get_logger().info(f"Control Mode Updated: {msg.data}") + def destroy_node(self): """ Override the destroy_node method to clean up resources used by the handler. diff --git a/teensy/src/Control Tasks/ServoControlTask.cpp b/teensy/src/Control Tasks/ServoControlTask.cpp index 8fd361b..a7cdeb7 100644 --- a/teensy/src/Control Tasks/ServoControlTask.cpp +++ b/teensy/src/Control Tasks/ServoControlTask.cpp @@ -5,19 +5,27 @@ ServoControlTask::ServoControlTask() sail_servo.attach(constants::servo::SAIL_PIN); rudder_servo.attach(constants::servo::RUDDER_PIN); // Set initial servo positions to 0-degrees - actuate_servo(sail_servo, 1050); - actuate_servo(rudder_servo, 1050); + actuate_servo(sail_servo, 1350); + actuate_servo(rudder_servo, rudder_to_pwm(0)); } void ServoControlTask::execute() { - // check if we have new serial data to update servos - if (sfr::serial::update_servos) + uint8_t sail_angle = sfr::serial::buffer[0]; + uint8_t rudder_angle = sfr::serial::buffer[1]; + + if (sfr::serial::autonomous_mode) { - uint8_t sail_angle = sfr::serial::buffer[0]; - uint8_t rudder_angle = sfr::serial::buffer[1]; + uint8_t auto_sail_angle = trim_sail(); + sfr::servo::sail_angle = auto_sail_angle; + sfr::servo::sail_pwm = sail_to_pwm(auto_sail_angle); + // actuate sail servo + actuate_servo(sail_servo, sfr::servo::sail_pwm); + } - // update sfr values based on incoming serial data if checks pass + else + { + // update sfr values based on incoming serial data if it checks pass if (sail_angle >= constants::servo::SAIL_MIN_ANGLE && sail_angle <= constants::servo::SAIL_MAX_ANGLE) { sfr::servo::sail_angle = sail_angle; @@ -26,36 +34,50 @@ void ServoControlTask::execute() // actuate sail servo actuate_servo(sail_servo, sfr::servo::sail_pwm); } + } - if (rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && rudder_angle <= constants::servo::RUDDER_MAX_ANGLE) + if (rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && rudder_angle <= constants::servo::RUDDER_MAX_ANGLE) + if (sfr::servo::rudder_angle >= constants::servo::RUDDER_MIN_ANGLE && sfr::servo::rudder_angle <= constants::servo::RUDDER_MAX_ANGLE) { sfr::servo::rudder_angle = rudder_angle; - sfr::servo::rudder_pwm = tail_to_pwm(rudder_angle); + sfr::servo::rudder_pwm = rudder_to_pwm(sfr::servo::rudder_angle); // actuate rudder servo actuate_servo(rudder_servo, sfr::servo::rudder_pwm); } - sfr::serial::update_servos = false; // reset flag for next update - } + sfr::serial::update_servos = false; // reset flag for next update } -// uint32_t ServoControlTask::angle_to_pwm(uint8_t angle) -// { -// // FIXME: this is hardcoded for our specific scenario -// return angle * 2; -// } - -uint32_t ServoControlTask::tail_to_pwm(uint8_t angle) +uint32_t ServoControlTask::sail_to_pwm(uint8_t angle) { - return angle * 2; + return map(angle, 0, 90, 1350, 1050); } -uint32_t ServoControlTask::sail_to_pwm(uint8_t angle) +uint32_t ServoControlTask::rudder_to_pwm(uint8_t angle) { - return map(angle, 0, 90, 1050, 1200); + return map(angle, -30, 30, 1325, 1675); } void ServoControlTask::actuate_servo(Servo &servo, uint32_t pwm) { servo.write(pwm); +} + +uint8_t ServoControlTask::trim_sail() +{ + uint16_t normal_wind_angle = sfr::anemometer::wind_angle; + normal_wind_angle = 180 - abs(normal_wind_angle - 180); // Normalized wind angle to 0 - 180 always because we only care about one side + + if ((normal_wind_angle >= 0 && normal_wind_angle <= 10)) + { + return 0; + } + else if (normal_wind_angle >= 150) + { + return 90; + } + else + { + return map(normal_wind_angle, 10, 150, 0, 90); + } } \ No newline at end of file diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index cdbdb4b..90f831e 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -27,6 +27,7 @@ void SerialMonitor::execute() sfr::serial::update_servos = true; sfr::servo::sail_angle = sfr::serial::buffer[0]; sfr::servo::rudder_angle = sfr::serial::buffer[1]; + sfr::serial::autonomous_mode = sfr::serial::buffer[2]; } else if (packet_started) { diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index 66fd0e7..7215b54 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -10,8 +10,8 @@ namespace sfr { uint8_t sail_angle = 0; uint8_t rudder_angle = 0; - uint32_t sail_pwm = 0; // FIXME: is this a PWM value? set to whatever corresponds to angle of 0 - uint32_t rudder_pwm = 0; // FIXME: is this a PWM value? set to whatever corresponds to angle of 0 + uint32_t sail_pwm = 0; + uint32_t rudder_pwm = 0; } namespace serial { @@ -19,6 +19,6 @@ namespace sfr uint8_t dropped_packets = 0; - uint8_t buffer[2] = {0}; + uint8_t buffer[3] = {0}; } } \ No newline at end of file diff --git a/teensy/src/sfr.hpp b/teensy/src/sfr.hpp index d274335..90c45c7 100644 --- a/teensy/src/sfr.hpp +++ b/teensy/src/sfr.hpp @@ -22,9 +22,10 @@ namespace sfr { extern bool update_servos; extern bool send_telemetry; + extern bool autonomous_mode; extern uint8_t dropped_packets; - extern uint8_t buffer[2]; + extern uint8_t buffer[3]; } } \ No newline at end of file From 3caeefc25fc4cb0c95b146be2c7454f50318734a Mon Sep 17 00:00:00 2001 From: Wang Mak Date: Sat, 12 Apr 2025 16:43:58 -0400 Subject: [PATCH 2/3] Anemometer check --- teensy/platformio.ini | 4 ++++ teensy/src/Control Tasks/ServoControlTask.hpp | 4 +++- teensy/src/MainControlLoop.cpp | 4 ++-- teensy/src/Monitors/SerialMonitor.cpp | 2 -- teensy/src/sfr.cpp | 2 ++ 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/teensy/platformio.ini b/teensy/platformio.ini index 68bfb5d..6dd2648 100644 --- a/teensy/platformio.ini +++ b/teensy/platformio.ini @@ -9,6 +9,10 @@ ; https://docs.platformio.org/page/projectconf.html [env:teensy40] + + + + platform = teensy@4.18 board = teensy40 framework = arduino diff --git a/teensy/src/Control Tasks/ServoControlTask.hpp b/teensy/src/Control Tasks/ServoControlTask.hpp index 6a5e281..5db7dcf 100644 --- a/teensy/src/Control Tasks/ServoControlTask.hpp +++ b/teensy/src/Control Tasks/ServoControlTask.hpp @@ -13,9 +13,11 @@ class ServoControlTask private: // uint32_t angle_to_pwm(uint8_t angle); uint32_t sail_to_pwm(uint8_t angle); - uint32_t tail_to_pwm(uint8_t angle); + uint32_t rudder_to_pwm(uint8_t angle); void actuate_servo(Servo &servo, uint32_t pwm); + + uint8_t trim_sail(); Servo sail_servo; Servo rudder_servo; diff --git a/teensy/src/MainControlLoop.cpp b/teensy/src/MainControlLoop.cpp index 7fbf197..692c802 100644 --- a/teensy/src/MainControlLoop.cpp +++ b/teensy/src/MainControlLoop.cpp @@ -1,7 +1,7 @@ #include "MainControlLoop.hpp" MainControlLoop::MainControlLoop() - : //anemometer_monitor(), + : anemometer_monitor(), serial_monitor(), servo_control_task(), serial_control_task() @@ -17,7 +17,7 @@ MainControlLoop::MainControlLoop() void MainControlLoop::execute() { - //anemometer_monitor.execute(); + anemometer_monitor.execute(); serial_monitor.execute(); servo_control_task.execute(); serial_control_task.execute(); diff --git a/teensy/src/Monitors/SerialMonitor.cpp b/teensy/src/Monitors/SerialMonitor.cpp index 90f831e..19d5f31 100644 --- a/teensy/src/Monitors/SerialMonitor.cpp +++ b/teensy/src/Monitors/SerialMonitor.cpp @@ -14,14 +14,12 @@ void SerialMonitor::execute() // only set flags if packet start byte is correct if (incoming_byte == constants::serial::RX_START_FLAG) { - Serial.println("reached serial monitor task 17"); packet_started = true; buffer_index = 0; } // check if packet end byte is correct and buffer is full else if (incoming_byte == constants::serial::RX_END_FLAG && buffer_index == sizeof(sfr::serial::buffer) && packet_started) { - Serial.println("serial monitor 24"); buffer_index = 0; packet_started = false; sfr::serial::update_servos = true; diff --git a/teensy/src/sfr.cpp b/teensy/src/sfr.cpp index 7215b54..f0af872 100644 --- a/teensy/src/sfr.cpp +++ b/teensy/src/sfr.cpp @@ -17,6 +17,8 @@ namespace sfr { bool update_servos = false; + bool autonomous_mode = false; + uint8_t dropped_packets = 0; uint8_t buffer[3] = {0}; From 8eaaa689e9e638de53c188f1e0b7ad05a4b9dfbe Mon Sep 17 00:00:00 2001 From: asun121 Date: Sat, 12 Apr 2025 16:54:38 -0400 Subject: [PATCH 3/3] Fixed fake teensy with teensy sail --- src/sailboat_launch/config/config_sim.yaml | 2 +- src/sailboat_main/sailboat_main/teensy/teensy_fake.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sailboat_launch/config/config_sim.yaml b/src/sailboat_launch/config/config_sim.yaml index f871231..96d9d86 100644 --- a/src/sailboat_launch/config/config_sim.yaml +++ b/src/sailboat_launch/config/config_sim.yaml @@ -15,7 +15,7 @@ sailbot: #Namespace teensy: ros__parameters: - teensy_port: "COM9" + teensy_port: "/dev/serial/by-id/usb-Teensyduino_USB_Serial_16089010-if00" simulated: true radio: diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py index 2025789..34ac896 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_fake.py +++ b/src/sailboat_main/sailboat_main/teensy/teensy_fake.py @@ -46,7 +46,7 @@ def _generate_random_wind(self): return int(self.wind_angle) - def send_command(self, sail, rudder): + def send_command(self, sail, rudder, mode): """ Send a properly formatted command packet to the servo.