Trim Sail & Control Mode Packet Implementation

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:

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)

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.
 

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.

teensy/platformio.ini

@@ -9,6 +9,10 @@
 ; https://docs.platformio.org/page/projectconf.html
 
 [env:teensy40]
+
+
+
+
 platform = teensy@4.18
 board = teensy40
 framework = arduino

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);
+    }
 }

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;

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();

teensy/src/Monitors/SerialMonitor.cpp

@@ -14,19 +14,18 @@ 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;
             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)
         {

teensy/src/sfr.cpp

@@ -10,15 +10,17 @@ 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
     {
         bool update_servos = false;
 
+        bool autonomous_mode = false;
+
         uint8_t dropped_packets = 0;
 
-        uint8_t buffer[2] = {0};
+        uint8_t buffer[3] = {0};
     }
 }

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];
     }
 }