diff --git a/src/sailboat_launch/config/config.yaml b/src/sailboat_launch/config/config.yaml index 4c953e0..5b43ce7 100644 --- a/src/sailboat_launch/config/config.yaml +++ b/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 diff --git a/src/sailboat_launch/config/config_sim.yaml b/src/sailboat_launch/config/config_sim.yaml index 167e9ce..91d1518 100644 --- a/src/sailboat_launch/config/config_sim.yaml +++ b/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 diff --git a/src/sailboat_main/sailboat_main/main_algo/main_algo.py b/src/sailboat_main/sailboat_main/main_algo/main_algo.py index 45a9959..771cd6d 100644 --- a/src/sailboat_main/sailboat_main/main_algo/main_algo.py +++ b/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: """ diff --git a/src/sailboat_main/sailboat_main/teensy/teensy_node.py b/src/sailboat_main/sailboat_main/teensy/teensy_node.py index 91f8021..7a6b272 100644 --- a/src/sailboat_main/sailboat_main/teensy/teensy_node.py +++ b/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 diff --git a/src/sailboat_main/sailboat_main/trim_sail/trim_sail.py b/src/sailboat_main/sailboat_main/trim_sail/trim_sail.py index ba4bfc5..3bd116b 100644 --- a/src/sailboat_main/sailboat_main/trim_sail/trim_sail.py +++ b/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