diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index ad647ab..c94fc12 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -352,7 +352,7 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): # 放宽判断标准 return abs(distance_moved - abs(distance_to_move)) < 0.15 # 如果误差小于15厘米，则认为成功 -def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, +def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, target_distance=0.2, pass_align=False, radius=None, scan_qrcode=False, @@ -365,8 +365,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc ctrl: Robot_Ctrl对象 msg: robot_control_cmd_lcmt对象 target_distance: 横线前目标保持距离，默认0.5米 - angle_deg: 旋转角度，支持90或180度 - left: True为左转，False为右转 + angle_deg: 旋转角度，正值为左转，负值为右转 pass_align: 是否跳过对准步骤，默认为False observe: 是否打印调试信息 scan_qrcode: 是否在旋转过程中扫描QR码，默认为False @@ -440,9 +439,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 3. 计算圆弧运动参数 # 根据角度大小调整时间补偿系数 - if angle_deg <= 70: + abs_angle_deg = abs(angle_deg) + if abs_angle_deg <= 70: time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数 - elif angle_deg <= 90: + elif abs_angle_deg <= 90: time_compensation = 0.85 # 90度左右使用稍大的系数 else: # 对180度旋转，增加补偿系数，使旋转时间更长 @@ -450,11 +450,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 调整实际目标角度，针对不同角度应用不同的缩放比例 actual_angle_deg = angle_deg - if angle_deg <= 70: + if abs_angle_deg <= 70: actual_angle_deg = angle_deg * 0.85 # 70度及以下角度减少到85% - elif angle_deg <= 90: + elif abs_angle_deg <= 90: actual_angle_deg = angle_deg * 0.9 # 90度减少到90% - elif angle_deg >= 170: # 处理大角度旋转，特别是180度情况 + elif abs_angle_deg >= 170: # 处理大角度旋转，特别是180度情况 actual_angle_deg = angle_deg # 对于180度旋转，使用完整角度，不进行缩放 if observe and actual_angle_deg != angle_deg: @@ -462,9 +462,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc angle_rad = math.radians(actual_angle_deg) - # 设定角速度（rad/s），可根据实际调整 + # 设定角速度（rad/s），角度的符号决定了转向方向 base_w = 0.8 - w = base_w if left else -base_w # 左转为正，右转为负 + w = math.copysign(base_w, angle_deg) # 保持角速度与角度同号 # 确保r有一个最小值，避免速度过低 min_radius = 0.2 # 最小半径，单位：米 @@ -512,8 +512,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 计算圆心位置 - 相对于机器人前进方向的左侧或右侧 # 圆心与机器人当前位置的距离就是圆弧半径 - center_x = start_position[0] - effective_r * math.sin(yaw) * (1 if left else -1) - center_y = start_position[1] + effective_r * math.cos(yaw) * (1 if left else -1) + # 角度为正(左转)时，圆心在左侧；角度为负(右转)时，圆心在右侧 + circle_side = 1 if angle_deg > 0 else -1 + center_x = start_position[0] - effective_r * math.sin(yaw) * circle_side + center_y = start_position[1] + effective_r * math.cos(yaw) * circle_side center_z = start_position[2] if len(start_position) > 2 else 0.0 # 在圆心放置绿色标记 @@ -522,10 +524,10 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 计算目标点的位置 - 旋转后的位置 # 使用几何关系：目标点是从起点绕圆心旋转angle_rad弧度后的位置 - target_angle = yaw + (angle_rad if left else -angle_rad) + target_angle = yaw + angle_rad # 直接使用angle_rad的正负确定方向 # 计算从圆心到目标点的向量 - target_x = center_x + effective_r * math.sin(target_angle) * (1 if left else -1) - target_y = center_y - effective_r * math.cos(target_angle) * (1 if left else -1) + target_x = center_x + effective_r * math.sin(target_angle) * circle_side + target_y = center_y - effective_r * math.cos(target_angle) * circle_side target_z = start_position[2] if len(start_position) > 2 else 0.0 # 在目标点放置红色标记 @@ -544,26 +546,26 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc current_w = w # 对小角度提前开始减速 - if angle_deg <= 70: + if abs_angle_deg <= 70: slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速 - elif angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机 + elif abs_angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机 slowdown_threshold = 0.7 # 当达到目标角度的70%时才开始减速 else: slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%） # 根据角度大小调整旋转停止阈值 - if angle_deg >= 170: + if abs_angle_deg >= 170: rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转 else: rotation_completion_threshold = 0.95 # 默认旋转到95%时停止 # 监控旋转进度并自行控制减速 # 对于180度旋转使用特殊处理 - if angle_deg >= 170: + if abs_angle_deg >= 170: # 简化旋转逻辑，直接持续旋转一段时间 rotation_time = t # 使用计算的理论时间 if observe: - info(f"大角度旋转({angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转") + info(f"大角度旋转({abs_angle_deg}度)，将持续旋转 {rotation_time:.2f} 秒", "旋转") # 发送固定时间的旋转命令 start_time = time.time() @@ -676,7 +678,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 当完成一定比例时开始减速 if completion_ratio > slowdown_threshold: # 实现多阶段减速，特别针对大角度旋转 - if angle_deg >= 170: + if abs_angle_deg >= 170: if completion_ratio < 0.7: # 第一阶段：60%-70% speed_factor = 0.65 # 原来是0.7，减少到0.65 elif completion_ratio < 0.8: # 第二阶段：70%-80% @@ -707,7 +709,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc ctrl.Send_cmd(msg) # 针对180度旋转，在减速命令后稍作等待，以确保减速效果 - if angle_deg >= 170: + if abs_angle_deg >= 170: time.sleep(0.03) # 短暂延迟，让减速命令更有效 if observe and time.time() % 0.5 < 0.02: # 每0.5秒左右打印一次 @@ -758,11 +760,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc final_angle_deg = math.degrees(final_angle_turned) - # 这里使用原始目标角度进行比较 + # 这里直接使用原始目标角度 if not bad_big_angle_corret: - target_angle_deg = angle_deg if left else -angle_deg + target_angle_deg = angle_deg else: - target_angle_deg = -angle_deg if left else angle_deg + target_angle_deg = -angle_deg if observe: # 输出停止过程中角度变化 @@ -779,7 +781,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 针对不同角度使用不同的微调阈值 adjustment_threshold = 3.0 # 默认微调阈值 - if angle_deg >= 170: + if abs_angle_deg >= 170: adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确 if angle_error > adjustment_threshold: # 如果误差超过阈值 @@ -790,9 +792,9 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc adjust_angle = target_angle_deg - final_angle_deg # 增加补偿系数，因为实际旋转常会超出理论计算值 - if angle_deg <= 70: + if abs_angle_deg <= 70: compensation_factor = 0.55 # 比之前更保守，从0.65降至0.55 - elif angle_deg >= 170: # 单独处理180度附近的大角度 + elif abs_angle_deg >= 170: # 单独处理180度附近的大角度 # 动态调整补偿因子，更保守处理 if angle_error > 60: # 如果误差超过60度 compensation_factor = 0.4 # 从0.5降至0.4 @@ -804,7 +806,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc compensation_factor = 0.6 # 从0.7降至0.6 # 对于特别大的误差进行更保守的单次校准 - if angle_deg >= 170 and angle_error > 30: + if abs_angle_deg >= 170 and angle_error > 30: # 大误差直接一步校准，使用更保守的补偿系数 adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15% adjust_angle = adjust_angle * adjusted_compensation @@ -1125,9 +1127,9 @@ def follow_dual_tracks(ctrl, msg, speed=0.5, max_time=30, target_distance=None, if __name__ == "__main__": move_to_hori_line(None, None, observe=True) # 90度左转 - arc_turn_around_hori_line(None, None, angle_deg=90, left=True, observe=True) + arc_turn_around_hori_line(None, None, angle_deg=90, observe=True) # 180度右转 - arc_turn_around_hori_line(None, None, angle_deg=180, left=False, observe=True) + arc_turn_around_hori_line(None, None, angle_deg=-90, observe=True) # 双轨道跟随 follow_dual_tracks(None, None, observe=True) \ No newline at end of file diff --git a/task_1/task_1.py b/task_1/task_1.py index 210cefc..380aad7 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -29,8 +29,7 @@ def run_task_1(ctrl, msg): turn_success, res = arc_turn_around_hori_line( ctrl=ctrl, msg=msg, - angle_deg=85, - left=False, + angle_deg=-85, # 负值表示右转 observe=observe, scan_qrcode=True, # 启用 QR 码扫描 qr_check_interval=0.3 # 每 0.3 秒检查一次扫描结果 @@ -50,10 +49,8 @@ def run_task_1(ctrl, msg): turn_success, res = arc_turn_around_hori_line( ctrl=ctrl, msg=msg, - angle_deg=170, + angle_deg=-170, # 负值表示右转 target_distance=0.5, # TODO 优化这里的参数 - left=direction, - # pass_align=True, observe=observe, # TODO clear @@ -95,8 +92,7 @@ def run_task_1(ctrl, msg): turn_success, res = arc_turn_around_hori_line( ctrl=ctrl, msg=msg, - angle_deg=-85, - left=True, + angle_deg=85, # 正值表示左转 observe=observe )