Lake test 4

src/sailboat_launch/config/config.yaml

@@ -5,7 +5,7 @@
 sailbot: #Namespace
    teensy:
       ros__parameters:
-         teensy_port: "/dev/serial/by-id/usb-Teensyduino_USB_Serial_16089010-if00"
+         teensy_port: "/dev/serial/by-id/usb-Teensyduino_USB_Serial_15675220-if00"
          simulated: false
 
    vectornav:
@@ -19,7 +19,7 @@ sailbot: #Namespace
 
    main_algo:
       ros__parameters:
-         tacking_buffer: 10
+         tacking_buffer: 20
          debug: True
 
 

src/sailboat_launch/config/config_sim.yaml

@@ -24,6 +24,6 @@ sailbot: #Namespace
 
    main_algo:
       ros__parameters:
-         tacking_buffer: 1
+         tacking_buffer: 15
          debug: True
 

src/sailboat_main/sailboat_main/main_algo/main_algo.py

@@ -96,7 +96,7 @@ class MainAlgo(Node):
     def __init__(self):
         super().__init__('main_algo')
 
-        self.declare_parameter('timer_period', 0.500) 
+        self.declare_parameter('timer_period', 0.2500) 
         self.timer_period = self.get_parameter('timer_period').value
 
         self.declare_parameter('tacking_buffer', 15)
@@ -105,9 +105,12 @@ def __init__(self):
         self.declare_parameter("debug", False)
         self.debug = self.get_parameter("debug").value
 
-        self.declare_parameter("no_go_zone", 45)
+        self.declare_parameter("no_go_zone", 50)
         self.no_go_zone = self.get_parameter("no_go_zone").value
 
+        self.declare_parameter("neutral_zone", 20)
+        self.neutral_zone = self.get_parameter("neutral_zone").value
+
         self.tack_time_tracker = 0
 
         #Subscription for current location
@@ -294,15 +297,26 @@ def calculate_rudder_angle(self):
         self.get_logger().info(f'Heading Difference: {diff}')
         self.diff = diff
 
-        if(not self.in_nogo()):
+        # if(not self.in_nogo() or np.absolute(self.neutral_zone) < 10 ):
+        #     rudder_angle = (diff / 180.0) * 25
+        # else:
+        #     rudder_angle = np.sign(diff) * 25 # max rudder angle if we are in nogo zone
+
+        # THIS RUDDER LOGIC IS TEMPORARY
+        # This will set the rudder to a small angle in the neutral zone, and a large angle in the no-go zone and medium angle elsewhere
+        if(np.absolute(diff) < self.neutral_zone ):
             rudder_angle = (diff / 180.0) * 25
-        else:
+        elif(self.in_nogo()):
             rudder_angle = np.sign(diff) * 25 # max rudder angle if we are in nogo zone
+        else:
+            rudder_angle = (diff / 180.0) * 25
+
+           # rudder_angle = np.sign(diff) * 15 # medium rudder angle otherwise (this will help maintain speed pre tack)
 
         self.get_logger().info(f'Rudder Angle Raw: {rudder_angle}')
 
         # Assuming rudder_angle is a floating-point number and you want it to be an Int32 message
-        rudder_angle = np.floor(rudder_angle / 5) * 5  # Floor the angle to the nearest multiple of 5
+        rudder_angle = np.floor(rudder_angle)  # Floor the angle
         rudder_angle = int(rudder_angle)  # Convert to int since Int32 requires an integer value
 
         self.get_logger().info(f'Rudder Angle: {rudder_angle}')
@@ -321,6 +335,23 @@ def in_nogo(self):
         if self.wind_dir is None:
             return False
         return (150 < self.wind_dir < 210)
+
+    def waypoint_in_nogo(self):
+        """
+        Check if the waypoint is in nogo zone based on the wind direction
+        """
+        if self.curr_dest is None or self.wind_dir is None or self.curr_loc is None:
+            return False
+
+        x_distance = self.curr_dest.easting - self.curr_loc.easting
+        y_distance = self.curr_dest.northing - self.curr_loc.northing
+
+        target_bearing = np.arctan2(y_distance, x_distance) * 180 / np.pi
+        diff = np.mod(self.heading_dir - target_bearing + 180, 360) - 180
+
+        self.get_logger().info(f'NOGO Zone Check: {diff}')
+
+        return ((self.wind_dir - self.no_go_zone) % 360 < diff < (self.wind_dir + self.no_go_zone) % 360)
 
     def calculateTP(self) -> UTMPoint:
         """

src/sailboat_main/sailboat_main/teensy/teensy_node.py

@@ -26,7 +26,7 @@ def __init__(self):
         self.declare_parameter('simulated', False)
 
         # teensy sends data every 0.5s, chose .25s read period to avoid overflow   
-        self.declare_parameter('rx_period', 0.500) 
+        self.declare_parameter('rx_period', 0.2500) 
 
         # get parameters
         self.timer_period = self.get_parameter('rx_period').value

src/sailboat_main/sailboat_main/trim_sail/trim_sail.py

@@ -46,10 +46,10 @@ def setSail(self, windDir):
         # sets a 20 degree buffer zone so that the sail does not always flip.
         if 0 <= windDir < 10 or 350 < windDir < 360:
             return 90
-        elif 210 < windDir <= 350:
-            return round(((7/15)*windDir - 80)/5)*5
-        elif 10 <= windDir < 150:
-            return round(((7/15)*windDir - 88)/5)*5
+        elif 225 < windDir <= 350:
+            return round(((18/25)*windDir - 162)/5)*5
+        elif 10 <= windDir < 135:
+            return round((-(18/25)*windDir + 97.2)/5)*5 # these were calculated by finding the line of the linear map for the sail angle
         # no go zone (150 <= cWindDir <= 210)
         else:
             return 0