ControlDemo_improve.py

"""
ControlDemo.py
优化后的UUV控制逻辑，集成A*路径安全偏移、障碍物膨胀和路径平滑功能，支持提前切换目标深度和深度稳定漂移控制
"""

import time
import math
import csv
from typing import List, Tuple
from pythondll import UnityDLLWrapper
from PIDAm import PIDAm

class ControlDemo:
    def densify_segment(self, segment: List[Tuple[float, float]], step: float = 3.0) -> List[Tuple[float, float]]:
        dense_segment = []
        for i in range(len(segment) - 1):
            x0, y0 = segment[i]
            x1, y1 = segment[i + 1]
            distance = math.hypot(x1 - x0, y1 - y0)
            num_steps = max(int(distance / step), 1)
            for j in range(num_steps):
                t = j / num_steps
                x = x0 + t * (x1 - x0)
                y = y0 + t * (y1 - y0)
                dense_segment.append((x, y))
        dense_segment.append(segment[-1])
        return dense_segment

    def __init__(self):
        self.dll = UnityDLLWrapper()
        self.start_time = time.time()
        self.last_print_time = time.time()
        self.print_interval = 1.0

        self.Rp = 6.0
        self.Ri = 0.02
        self.Rd = 0.3

        self.Vp = 1.3
        self.Vi = 0.01
        self.Vd = 0.4

        self.Ep = 1.2
        self.Ei = 0.08
        self.Ed = 0.6

        self.UUV1HRpid = PIDAm(4.5 * self.Rp, self.Ri, self.Rd)
        self.UUV1VRpid = PIDAm(self.Vp, self.Vi, self.Vd)
        self.UUV1Epid = PIDAm(self.Ep, self.Ei, self.Ed)

        path_before = [(40, 700), (151, 441),(310,415)]
        cdef = [(375,300),(430,220),(320,220),(370,300)]
        path_after = [
            (395, 357.5),
            (420,415),(560,415),
            (560, 470), (560, 580), (410, 580), (410, 450), (560, 450),
            (587,373), (665, 318), (826, 260), (852, 293),(980,242)
        ]
        self.original_path = path_before + cdef + path_after
        self.pathpoint = self.original_path.copy()
        self.pathpoint_index = 0

        self.trajectory_log = []

        self.depth_target = -15
        self.depth_hold_mode = "pid_active"
        self.depth_last_out_time = None

    def set_team_name(self, name: str) -> None:
        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 _update_depth_target(self, current_pos: Tuple[float, float]):
        depth_segments = [ (0, -15),(2, -20),(7, -30), (10, -20),   ]

        for index, depth in reversed(depth_segments):
            if self.pathpoint_index >= index:
                target_pos = self.pathpoint[index]
                distance = math.hypot(current_pos[0] - target_pos[0], current_pos[1] - target_pos[1])
                if distance < 25 or self.pathpoint_index > index:
                    self.depth_target = depth
                    break

    def uuv1_control(self):
        elapsed_time = time.time() - self.start_time
        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()
        vertical_velocity = self.dll.get_vertical_velocity()

        direction_vector = (target_x - current_x, target_y - current_y)
        cos_theta = self._dot((1, 0), direction_vector) / (self._norm((1, 0)) * self._norm(direction_vector) + 1e-6)
        sin_theta = self._cross((1, 0), direction_vector) / (self._norm((1, 0)) * self._norm(direction_vector) + 1e-6)
        target_heading = math.degrees(math.atan2(sin_theta, cos_theta))

        vertical_rudder = self.UUV1VRpid.pid_realize(0.01, target_heading, current_heading, is_angle=True)
        engines = self.UUV1Epid.pid_realize(0.01, 1.0, math.hypot(current_vel_x, current_vel_y))

        # ------ 深度控制逻辑 ------
        self._update_depth_target((current_x, current_y))
        depth_error = abs(current_z - self.depth_target)
        margin = 2.0
        now = time.time()

        if self.depth_hold_mode == "pid_active":
            if depth_error < margin:
                self.depth_hold_mode = "drift"
                self.depth_last_out_time = None
        elif self.depth_hold_mode == "drift":
            if depth_error > margin:
                if self.depth_last_out_time is None:
                    self.depth_last_out_time = now
                elif now - self.depth_last_out_time > 0.25:
                    self.depth_hold_mode = "reentry"
                    self.depth_last_out_time = None
        else:
            if depth_error < margin:
                self.depth_hold_mode = "drift"

        if self.depth_hold_mode in ("pid_active", "reentry"):
            horizontal_rudder = self.UUV1HRpid.pid_realize(0.01, self.depth_target, current_z)
        else:
            horizontal_rudder = 0.0

        # ------ 目标点推进逻辑 ------
        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
            print(f"""
         ===航行器状态===
         时间:{int(elapsed_time // 3600):02d}:{int((elapsed_time % 3600) // 60):02d}:{int(elapsed_time % 60):02d}
         位置:X={current_x:.2f}, Y={current_y:.2f}, 深度={current_z:.2f}
         航向角:{current_heading:.2f}°
         水平速度={math.hypot(current_vel_x, current_vel_y):.2f}, 垂直速度={vertical_velocity:.2f}
         当前目标点:{self.pathpoint_index}/{len(self.pathpoint)} ({target_x:.1f},{target_y:.1f})
         推进={engines:.2f}，水平舵={horizontal_rudder:.2f}（模式:{self.depth_hold_mode}）
         ===================""")
            self.last_print_time = current_time

        self.trajectory_log.append({
            "time": elapsed_time,
            "x": current_x,
            "y": current_y,
            "z": current_z,
            "heading": current_heading,
            "h_speed": math.hypot(current_vel_x, current_vel_y),
            "v_speed": vertical_velocity,
            "target_index": self.pathpoint_index,
            "target_x": target_x,
            "target_y": target_y
        })

    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
        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()
    with open("uuv_trajectory_log.csv", "w", newline='') as csvfile:
        fieldnames = ["time", "x", "y", "z", "heading", "h_speed", "v_speed", "target_index", "target_x", "target_y"]
        writer = csv.DictWriter(csvfile, fieldnames=fieldnames)
        writer.writeheader()
        writer.writerows(controller.trajectory_log)
    print("轨迹已保存为 uuv_trajectory_log.csv")