diff --git a/base_move/go_straight.py b/base_move/go_straight.py index aad4ca7..c5b9b56 100644 --- a/base_move/go_straight.py +++ b/base_move/go_straight.py @@ -184,16 +184,16 @@ def go_straight(ctrl, msg, distance, speed=0.5, observe=False): # 判断是否成功完成 distance_error = abs(actual_distance - abs_distance) - success = distance_error < 0.1 # 如果误差小于10厘米，则认为成功 + go_success = distance_error < 0.1 # 如果误差小于10厘米，则认为成功 if observe: info(f"目标距离: {abs_distance:.3f}米, 实际距离: {actual_distance:.3f}米, 误差: {distance_error:.3f}米", "距离") - if success: + if go_success: success(f"移动成功", "成功") else: warning(f"移动失败，误差过大: {distance_error:.3f}米", "失败") - return success + return go_success # 用法示例 if __name__ == "__main__": diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index b66d92c..ad647ab 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -230,7 +230,6 @@ def go_straight_by_hori_line(ctrl, msg, distance=0.5, speed=0.5, observe=False): go_straight(ctrl, msg, distance=distance, speed=speed, observe=observe) - def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): """ 控制机器人校准并移动到横向线前的指定距离 @@ -355,7 +354,9 @@ def move_to_hori_line(ctrl, msg, target_distance=0.5, observe=False): def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distance=0.2, pass_align=False, - scan_qrcode=False, qr_check_interval=0.3, + radius=None, + scan_qrcode=False, + qr_check_interval=0.3, observe=False, bad_big_angle_corret=False): """ @@ -410,32 +411,43 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc info("跳过对准步骤", "信息") # 2. 检测横线并计算距离 - image = ctrl.image_processor.get_current_image() - edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe, save_log=True) - if edge_point is None or edge_info is None: - error("无法检测到横向线，停止动作", "停止") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() - return False, res - return False, res - - camera_height = 0.355 # 单位: 米 - r = calculate_distance_to_line(edge_info, camera_height, observe=observe) + r = radius if r is None: - error("无法计算到横向线的距离，停止动作", "停止") - if scan_qrcode: - ctrl.image_processor.stop_async_scan() + image = ctrl.image_processor.get_current_image() + edge_point, edge_info = detect_horizontal_track_edge(image, observe=observe) + if edge_point is None or edge_info is None: + error("无法检测到横向线，停止动作", "停止") + if scan_qrcode: + ctrl.image_processor.stop_async_scan() + return False, res return False, res - return False, res - - # 减去目标距离 - r -= target_distance + + camera_height = 0.355 # 单位: 米 + r = calculate_distance_to_line(edge_info, camera_height, observe=observe) + if r is None: + error("无法计算到横向线的距离，停止动作", "停止") + if scan_qrcode: + ctrl.image_processor.stop_async_scan() + return False, res + return False, res + + # 减去目标距离 + r -= target_distance res['radius'] = r if observe: info(f"当前距离: {r:.3f}米", "距离") # 3. 计算圆弧运动参数 + # 根据角度大小调整时间补偿系数 + if angle_deg <= 70: + time_compensation = 0.78 # 对70度及以下角度使用更小的补偿系数 + elif angle_deg <= 90: + time_compensation = 0.85 # 90度左右使用稍大的系数 + else: + # 对180度旋转，增加补偿系数，使旋转时间更长 + time_compensation = 1.4 # 增加40%的时间 + # 调整实际目标角度，针对不同角度应用不同的缩放比例 actual_angle_deg = angle_deg if angle_deg <= 70: @@ -462,15 +474,19 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 这样才能保证走圆弧 v = abs(w * effective_r) # 线速度，正负号与角速度方向一致 + info(f"开始移动圆弧，半径: {effective_r:.3f}米, 线速度: {v:.3f}m/s, 角速度: {w:.3f}rad/s") if observe: if effective_r != r: warning(f"注意: 实际半径 {r:.3f}米 太小，使用最小半径 {effective_r:.3f}米 计算速度", "警告") t = abs(angle_rad / w) # 运动时间，取绝对值保证为正 + # 应用时间补偿系数 + t *= time_compensation + if observe: - debug(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s", "计算") - debug(f"理论运动时间: {t:.2f}s", "时间") + print(f"圆弧半径: {effective_r:.3f}米, 角速度: {w:.3f}rad/s, 线速度: {v:.3f}m/s") + print(f"理论运动时间: {abs(angle_rad / w):.2f}s, 应用补偿系数{time_compensation}后: {t:.2f}s") # 4. 发送圆弧运动指令 msg.mode = 11 @@ -531,13 +547,13 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc if angle_deg <= 70: slowdown_threshold = 0.65 # 当达到目标角度的65%时开始减速 elif angle_deg >= 170: # 针对大角度(如180度)采用更晚的减速时机 - slowdown_threshold = 0.8 # 当达到目标角度的70%时才开始减速，改为80% + slowdown_threshold = 0.7 # 当达到目标角度的70%时才开始减速 else: slowdown_threshold = 0.75 # 对于大角度，75%时开始减速（原来是80%） # 根据角度大小调整旋转停止阈值 if angle_deg >= 170: - rotation_completion_threshold = 0.92 # 大角度旋转提前停止，从0.85改为0.92 + rotation_completion_threshold = 0.85 # 大角度旋转提前停止，避免惯性导致过度旋转 else: rotation_completion_threshold = 0.95 # 默认旋转到95%时停止 @@ -553,7 +569,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc start_time = time.time() # 前段使用全速，但减少前段旋转时间比例，减少过度旋转 - first_segment_time = rotation_time * 0.75 # 原来是0.65，增加到0.75 + first_segment_time = rotation_time * 0.65 # 原来是0.7，减少到0.65 elapsed_time = 0 while elapsed_time < first_segment_time and time.time() - start_time < rotation_time + 2: # 加2秒保护 @@ -586,8 +602,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc if observe: info(f"进入减速阶段", "旋转") - # 减速到80%，保持前进速度和角速度的比例关系 - reduced_w = w * 0.8 # 原来是0.7，提高到0.8 + # 减速到70%，保持前进速度和角速度的比例关系 + reduced_w = w * 0.7 # 原来是0.5 reduced_v = abs(reduced_w * effective_r) # 维持圆弧关系 msg.mode = 11 @@ -598,8 +614,8 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc msg.life_count += 1 ctrl.Send_cmd(msg) - # 继续旋转直到总时间结束，增加总旋转时间 - total_rotation_time = rotation_time * 1.25 # 原来是1.15，增加到1.25 + # 继续旋转直到总时间结束，减少总旋转时间 + total_rotation_time = rotation_time * 1.15 # 原来是1.2，减少到1.15 while time.time() - start_time < total_rotation_time: # 计算已旋转角度（仅用于打印） if observe and time.time() % 0.5 < 0.02: @@ -662,11 +678,11 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 实现多阶段减速，特别针对大角度旋转 if angle_deg >= 170: if completion_ratio < 0.7: # 第一阶段：60%-70% - speed_factor = 0.75 # 原来是0.65，提高到0.75 + speed_factor = 0.65 # 原来是0.7，减少到0.65 elif completion_ratio < 0.8: # 第二阶段：70%-80% - speed_factor = 0.45 # 原来是0.35，提高到0.45 + speed_factor = 0.35 # 原来是0.4，减少到0.35 else: # 第三阶段：80%以上 - speed_factor = 0.25 # 原来是0.15，提高到0.25 + speed_factor = 0.15 # 原来是0.2，减少到0.15 else: # 标准减速逻辑保持不变 # 剩余比例作为速度系数 @@ -764,7 +780,7 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc # 针对不同角度使用不同的微调阈值 adjustment_threshold = 3.0 # 默认微调阈值 if angle_deg >= 170: - adjustment_threshold = 5.0 # 大角度旋转使用更大的微调阈值，从10.0改为5.0 + adjustment_threshold = 10.0 # 大角度旋转使用更大的微调阈值，因为初步旋转已经确保大致方向正确 if angle_error > adjustment_threshold: # 如果误差超过阈值 if observe: @@ -779,18 +795,18 @@ def arc_turn_around_hori_line(ctrl, msg, angle_deg=90, left=True, target_distanc elif angle_deg >= 170: # 单独处理180度附近的大角度 # 动态调整补偿因子，更保守处理 if angle_error > 60: # 如果误差超过60度 - compensation_factor = 0.45 # 从0.4升至0.45 + compensation_factor = 0.4 # 从0.5降至0.4 elif angle_error > 30: # 如果误差超过30度 - compensation_factor = 0.4 # 从0.35升至0.4 + compensation_factor = 0.35 # 从0.4降至0.35 else: - compensation_factor = 0.35 # 从0.25升至0.35，更积极 + compensation_factor = 0.25 # 从0.3降至0.25，更保守 else: compensation_factor = 0.6 # 从0.7降至0.6 - # 对于特别大的误差进行更积极的单次校准 + # 对于特别大的误差进行更保守的单次校准 if angle_deg >= 170 and angle_error > 30: - # 大误差直接一步校准，使用更积极的补偿系数 - adjusted_compensation = compensation_factor * 0.95 # 在已有补偿基础上只降低5% + # 大误差直接一步校准，使用更保守的补偿系数 + adjusted_compensation = compensation_factor * 0.85 # 在已有补偿基础上再降低15% adjust_angle = adjust_angle * adjusted_compensation if observe: diff --git a/main.py b/main.py index 43a76b0..29e1026 100644 --- a/main.py +++ b/main.py @@ -111,6 +111,9 @@ class Robot_Ctrl(object): # if self.odo_msg.timestamp % 100 == 0: # print(self.odo_msg.xyz) + def odo_reset(self): + self.calibration_offset = self.odo_msg.xyz + def rec_responce(self): while self.runing: self.lc_r.handle() diff --git a/task_1/task_1.py b/task_1/task_1.py index 48f540e..210cefc 100644 --- a/task_1/task_1.py +++ b/task_1/task_1.py @@ -43,15 +43,15 @@ def run_task_1(ctrl, msg): section('任务1-2：移动到横线', "移动") # 执行常规的移动操作，不需要 QR 码扫描 - move_to_hori_line(ctrl, msg, target_distance=1, observe=observe) + move_to_hori_line(ctrl, msg, target_distance=0.8, observe=observe) section('任务1-3：转弯', "旋转") - direction = True if res['qr_result'] == 'A-1' else False + direction = False # if res['qr_result'] == 'A-2' else True turn_success, res = arc_turn_around_hori_line( ctrl=ctrl, msg=msg, angle_deg=170, - target_distance=0.4, # TODO 优化这里的参数 + target_distance=0.5, # TODO 优化这里的参数 left=direction, # pass_align=True, @@ -59,11 +59,15 @@ def run_task_1(ctrl, msg): # TODO clear bad_big_angle_corret=True ) - - return + if turn_success: + success("任务1-3完成", "完成") + else: + warning("任务1-3失败", "警告") + return + section('任务1-4：直线移动', "移动") - move_distance = 0.4 + move_distance = 0.3 if direction: # TODO 这里的校准不一定好用，需要优化 go_straight_by_hori_line(ctrl, msg, distance=move_distance, speed=0.5, observe=observe) @@ -82,8 +86,7 @@ def run_task_1(ctrl, msg): ctrl.base_msg.stand_up() section('任务1-6：返回', "移动") - go_straight(ctrl, msg, distance=-move_distance + res['radius'], speed=0.5, observe=observe) - go_straight(ctrl, msg, distance=-move_distance - res['radius'], speed=0.5, observe=observe) + go_straight(ctrl, msg, distance=-(move_distance + res['radius']), speed=0.5, observe=observe) # turn and back turn_degree(ctrl, msg, degree=-90, absolute=True)