ControlDemo.py

"""
ControlDemo.py
优化后的UUV控制逻辑，集成A*路径安全偏移、障碍物膨胀和路径平滑功能
"""
import time
import math
from typing import List, Tuple, Optional
from pythondll import UnityDLLWrapper
from PIDAm import PIDAm

class ControlDemo:
    def __init__(self):
        # ------------------------ 初始化配置 ------------------------
        self.dll = UnityDLLWrapper()
        self.start_time = time.time()
        self.last_print_time = time.time()
        self.print_interval = 1.0

        # ------------------------ PID参数优化 ------------------------
        self.Rp = 0.3  # 增大水平舵比例系数
        self.Ri = 0.1
        self.Rd = 0.6
        self.Ep = 2 # 提高引擎灵敏度
        self.Ei = 0.3
        self.Ed = 0.6

        self.UUV1HRpid = PIDAm(4.5 * self.Rp, self.Ri, self.Rd)  # 水平舵
        self.UUV1VRpid = PIDAm(self.Rp, self.Ri, self.Rd)  # 垂直舵
        self.UUV1Epid = PIDAm(self.Ep, self.Ei, self.Ed)  # 引擎

        # ------------------------ 路径配置 ------------------------

        self.original_path = [
            #海工巡检区
            (45,700),(279,569),(340,423),(370,287),(412,224),
            (340,224),(370,287),(420,401),(531,438),(579,471),
            (572,617),(415,574),(440,475),(574,448),
            #岛屿区
            (587,499), (703, 448), (830, 436), (867, 361),
           (980,242)

        ]
        self.pathpoint = self.original_path.copy()
        self.pathpoint_index = 0
        self.request_depth = [-15, -20, -30]
        self.depth_index = 0
    def set_team_name(self, name: str) -> None:
        """设置团队名称（通过DLL接口）"""
        self.dll.set_team_name(name)

    def process_path(self):
        #用于在文件间传递设定的静态路径
        return self.pathpoint

    def _calculate_path_angles(self, path: List[Tuple]) -> List[float]:
        """计算路径点行进方向角度"""
        angles = []
        for i in range(len(path) - 1):
            dx = path[i + 1][0] - path[i][0]
            dy = path[i + 1][1] - path[i][1]
            angles.append(math.atan2(dy, dx))
        angles.append(angles[-1])  # 最后一个点沿用前一个角度
        return angles

    # --------------- 原有控制逻辑保持不变 ---------------
    def uuv1_control(self):

        # ------------------------ 获取目标位置和当前状态 ------------------------
        target_x, target_y = self.pathpoint[self.pathpoint_index]

        # ------------------------ 传感器数据获取 ------------------------
        current_x, current_y, current_z = self.dll.get_uuv1_pos()
        current_vel_x, current_vel_y, _ = self.dll.get_uuv1_horizontal_velocity()
        current_heading = self.dll.get_uuv1_horizontal_heading()
        water_speed, water_heading = self.dll.get_water_current()
        horizontal_angular_velocity = self.dll.get_horizontal_angular_velocity()
        vertical_angular_velocity = self.dll.get_vertical_angular_velocity()
        vertical_velocity = self.dll.get_vertical_velocity()
        # 航向角计算
        direction_vector = (target_x - current_x, target_y - current_y)
        world_vector = (1, 0)
        cos_theta = self._dot(world_vector, direction_vector) / (
                    self._norm(world_vector) * self._norm(direction_vector) + 1e-6)
        sin_theta = self._cross(world_vector, direction_vector) / (
                    self._norm(world_vector) * self._norm(direction_vector) + 1e-6)
        target_heading = math.degrees(math.atan2(sin_theta, cos_theta))

        # PID计算
        vertical_rudder = self.UUV1VRpid.pid_realize(
            0.01, target_heading, current_heading, is_angle=True)
        engines = self.UUV1Epid.pid_realize(
            0.005, math.hypot(*direction_vector),
            math.hypot(*self.dll.get_uuv1_horizontal_velocity()[:2]))
        horizontal_rudder = self.UUV1HRpid.pid_realize(
            0.005, self.request_depth[self.depth_index], current_z)
        # ------------------------ 速度计算 ------------------------
        target_horizontal_speed = math.hypot(target_x - current_x, target_y - current_y)
        current_h_speed = math.hypot(current_vel_x, current_vel_y)
        # ------------------------ 路径点更新逻辑 ------------------------
        if self.pathpoint_index == 7:
            self.depth_index = 1

        if self.pathpoint_index == 10:
            self.depth_index = 2

        if math.hypot(*direction_vector) < 20 and self.pathpoint_index < len(self.pathpoint) - 1:
            self.pathpoint_index += 1

        # ------------------------ 控制指令下发 ------------------------
        self.dll.set_uuv1_engines_control(engines)
        self.dll.set_uuv1_vertical_rudder(-vertical_rudder)
        self.dll.set_uuv1_horizontal_rudder(horizontal_rudder)

        # ------------------------ 状态打印 ------------------------
        current_time = time.time()
        if current_time - self.last_print_time >= self.print_interval:
            elapsed_time = current_time - self.start_time
            hours = int(elapsed_time // 3600)
            minutes = int((elapsed_time % 3600) // 60)
            seconds = int(elapsed_time % 60)
            formatted_time = f"{hours:02d}:{minutes:02d}:{seconds:02d}"

            print(f"""
         ===航行器状态===
         时间（运动）:{formatted_time}
         位置:X={current_x:.6f}, Y={current_y:.6f}
         深度={current_z:.6f}
         速度:水平={current_h_speed:.6f}, 垂直={vertical_velocity:.6f}
         航向角:{current_heading:.6f}°
         目标点:{self.pathpoint_index + 1}/{len(self.pathpoint)}({target_x:.2f}, {target_y:.2f})
         ===================""")
            self.last_print_time = current_time

    # ------------------------ 数学工具方法 ------------------------
    def _dot(self, a: Tuple[float, float], b: Tuple[float, float]) -> float:
        """计算二维向量点积"""
        return a[0] * b[0] + a[1] * b[1]

    def _cross(self, a: Tuple[float, float], b: Tuple[float, float]) -> float:
        """计算二维向量叉积"""
        return a[0] * b[1] - a[1] * b[0]

    def _norm(self, a: Tuple[float, float]) -> float:
        """计算二维向量模长"""
        return math.sqrt(a[0] ** 2 + a[1] ** 2)

    def run(self):
        """主控制循环"""
        self.set_team_name("MyTeam")
        control_interval = 0.002  # 500Hz控制频率
        next_time = time.perf_counter()

        try:
            while True:
                if self.pathpoint_index <= len(self.pathpoint) - 1:
                    self.uuv1_control()
                else:
                     # 到达终点后停止执行器
                    self.dll.set_uuv1_engines_control(0.0)
                    self.dll.set_uuv1_horizontal_rudder(0.0)
                    self.dll.set_uuv1_vertical_rudder(0.0)
                    print("路径跟踪完成！")
                    break

                # 高精度时间控制
                next_time += control_interval
                sleep_time = next_time - time.perf_counter()
                if sleep_time > 0:
                    time.sleep(sleep_time)
        except KeyboardInterrupt:
            print("控制循环被用户中断")

if __name__ == "__main__":
    controller = ControlDemo()
    controller.run()