增加电机控制时间延迟

calculate/trajectory.py

@@ -16,6 +16,43 @@ def line_trajectory(start=(40, 0), end=(120, 0), num_points=20):
     y_list = start[1] + t * (end[1] - start[1])
     return x_list, y_list
 
+
+def line_trajectory_fix(start=(40, 0), end=(120, 100), vx=0.1, vy=0.1, num_points=20):
+    """
+    生成带速度分量的匀速斜线轨迹
+    参数：
+        start: 起始点 (x, y)
+        end: 终点 (x, y) —— 仅用于估算运行时间（可选）
+        vx: x方向速度（单位/秒）
+        vy: y方向速度（单位/秒）
+        num_points: 生成的轨迹点数
+    返回：
+        x_list, y_list: 轨迹坐标数组
+    """
+    # 速度大小
+    speed = np.sqrt(vx**2 + vy**2)
+    if speed == 0:
+        raise ValueError("速度不能为零")
+
+    # 估算从 start 到 end 的距离（用于估算总时间）
+    if end is not None:
+        dx = end[0] - start[0]
+        dy = end[1] - start[1]
+        distance = np.sqrt(dx**2 + dy**2)
+        total_time = distance / speed  # 理论到达时间
+        print(total_time)
+    else:
+        total_time = 10.0  # 默认运行10秒
+
+    # 时间序列：从 0 到 total_time，均匀分布 num_points 个点
+    t = np.linspace(0, total_time, num_points)
+
+    # 位置 = 起点 + 速度 × 时间
+    x_list = start[0] + vx * t
+    y_list = start[1] + vy * t
+
+    return x_list, y_list
+
 def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=60):
     """ 椭圆轨迹 """
     angles = np.linspace(0, 2 * np.pi, num_points)