diff --git a/base_move/move_base_hori_line.py b/base_move/move_base_hori_line.py index 57070a7..325c836 100644 --- a/base_move/move_base_hori_line.py +++ b/base_move/move_base_hori_line.py @@ -50,6 +50,7 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): error("未检测到横向线，无法进行校准", "失败") # 尝试小幅度摇头寻找横线 + # TODO 改成上下低头； if attempts < max_attempts - 1: small_angle = 5 * (1 if attempts % 2 == 0 else -1) info(f"尝试摇头 {small_angle}度 寻找横线", "校准") @@ -79,9 +80,6 @@ def align_to_horizontal_line(ctrl, msg, observe=False, max_attempts=3): if abs(angle_to_rotate) < 0.5: # 降低最小旋转阈值 angle_to_rotate = math.copysign(0.5, angle_to_rotate) - # 限制单次旋转角度 - angle_to_rotate = max(-15, min(15, angle_to_rotate)) - # 检查累积旋转 if abs(accumulated_angle + angle_to_rotate) > max_total_rotation: warning(f"累积旋转角度({accumulated_angle + angle_to_rotate:.2f}°)过大，限制旋转", "警告") diff --git a/base_move/turn_degree.py b/base_move/turn_degree.py index 19e9ca2..ccb18fb 100644 --- a/base_move/turn_degree.py +++ b/base_move/turn_degree.py @@ -45,202 +45,146 @@ def turn_degree(ctrl, msg, degree=90, absolute=False, precision=False): # 如果是精确模式，使用更小的误差阈值 limit = 0.03 if precision else 0.04 # 约1.7度或2.3度 - # 计算最短旋转方向和距离 - def circle_dist(target, location): - value1 = abs(target - location) - value2 = 2 * math.pi - value1 - direction1 = 1 if target > location else 0 # 1为逆时针，0为顺时针 - - # 计算两个方向哪个距离更短 - if value1 < value2: - return direction1, value1 - else: - return 1 - direction1, value2 + # 确定最短旋转方向 + remaining_dist = target_yaw - current_yaw + if remaining_dist > math.pi: + remaining_dist -= 2 * math.pi + elif remaining_dist < -math.pi: + remaining_dist += 2 * math.pi - # 获取旋转方向和距离 - direction, dist = circle_dist(target_yaw, current_yaw) - info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "旋转") + # 如果误差已经在允许范围内，无需旋转 + if abs(remaining_dist) <= limit: + success(f"当前角度误差已在允许范围内，无需旋转", "成功") + return True - if abs(dist) > limit: - # 主要转向 - const_int = 2470 # 转1.57弧度约需2470的duration值 - - # 精确模式下使用更小的旋转速度 - turn_speed = 0.3 if precision else 0.5 - - # 如果角度很小且使用精确模式，进一步降低速度 - if precision and abs(dist) < 0.2: # 约11.5度 - turn_speed = 0.2 - - # 设置转向命令 - msg.mode = 11 # Locomotion模式 - msg.gait_id = 26 # 自变频步态 - msg.vel_des = [0, 0, turn_speed if direction > 0 else -turn_speed] # 转向速度 - - # 精确模式下延长转向时间以保证稳定性 - duration_factor = 1.2 if precision else 1.0 - msg.duration = int(const_int * abs(dist) * duration_factor) - msg.step_height = [0.06, 0.06] # 抬腿高度 - msg.life_count += 1 - - # 发送命令 - ctrl.Send_cmd(msg) - debug(f"发送旋转命令：方向={'逆时针' if direction > 0 else '顺时针'}, 速度={turn_speed}, 持续时间={msg.duration}", "旋转") - - # 等待转向完成 - # 精确模式下增加等待时间 - wait_factor = 1.3 if precision else 1.0 - wait_time = 7 * abs(dist) / 1.57 * wait_factor - debug(f"等待旋转完成: {wait_time:.2f}秒", "时间") - - # 精确模式下使用实时监控而不是固定等待时间 - if precision and abs(dist) > 0.1: # 对于较大角度 - start_time = time.time() - last_yaw = current_yaw - stable_count = 0 - timeout = wait_time + 3 # 增加超时保护 - - # 实时监控旋转进度 - while time.time() - start_time < timeout: - time.sleep(0.1) # 频繁检查 - - current_yaw_now = ctrl.odo_msg.rpy[2] - # 计算已旋转角度 - rotated = abs(current_yaw_now - last_yaw) - if rotated > math.pi: - rotated = 2 * math.pi - rotated - - # 如果旋转速度很小（几乎停止） - if rotated < 0.01: # 约0.6度 - stable_count += 1 - else: - stable_count = 0 - - last_yaw = current_yaw_now - - # 如果机器人稳定一段时间，认为旋转完成 - if stable_count >= 3: - debug(f"检测到旋转已稳定，提前结束等待", "旋转") - break - - # 每0.5秒打印一次进度 - elapsed = time.time() - start_time - if int(elapsed * 2) % 2 == 0: - remaining_yaw = target_yaw - current_yaw_now - # 标准化到 [-pi, pi] - if remaining_yaw > math.pi: - remaining_yaw -= 2 * math.pi - if remaining_yaw < -math.pi: - remaining_yaw += 2 * math.pi - debug(f"旋转进度: {elapsed:.1f}s/{wait_time:.1f}s, 剩余角度: {math.degrees(remaining_yaw):.2f}°", "进度") + # 确定旋转方向和速度 + turn_direction = 1 if remaining_dist > 0 else -1 + + # 根据精确模式选择旋转速度 + if precision: + # 精确模式下，角度较大时使用中等速度，较小时使用慢速 + if abs(remaining_dist) > 0.5: # 大约28度以上 + turn_speed = 0.25 else: - # 非精确模式使用固定等待时间 - time.sleep(wait_time) + turn_speed = 0.15 + else: + # 非精确模式下，角度较大时使用快速，较小时使用中等速度 + if abs(remaining_dist) > 0.5: # 大约28度以上 + turn_speed = 0.5 + else: + turn_speed = 0.3 + + info(f"开始旋转: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°, 需旋转{math.degrees(abs(remaining_dist)):.2f}°", "旋转") + + # 发送旋转命令（持续旋转） + msg.mode = 11 # Locomotion模式 + msg.gait_id = 26 # 自变频步态 + msg.vel_des = [0, 0, turn_speed * turn_direction] # 转向速度和方向 + msg.duration = 30000 # 设置一个大的持续时间值，后面会主动停止 + msg.step_height = [0.06, 0.06] # 抬腿高度 + msg.life_count += 1 + + # 发送命令 + ctrl.Send_cmd(msg) + debug(f"开始持续旋转：方向={'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "旋转") + + # 等待并监测旋转进度 + start_time = time.time() + last_yaw = current_yaw + stable_count = 0 + + # 设置最大超时时间（秒）防止无限等待 + max_timeout = 15 if abs(remaining_dist) > 1.0 else 10 + + while True: + time.sleep(0.05) # 高频监测旋转状态 # 获取当前角度 - current_yaw = ctrl.odo_msg.rpy[2] - info(f"主要转向完成: 当前角度={math.degrees(current_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度") + current_yaw_now = ctrl.odo_msg.rpy[2] - # 计算剩余误差 - remaining_dist = target_yaw - current_yaw - if remaining_dist > math.pi: - remaining_dist -= 2 * math.pi - elif remaining_dist < -math.pi: - remaining_dist += 2 * math.pi + # 计算当前与目标的角度差 + current_error = target_yaw - current_yaw_now + if current_error > math.pi: + current_error -= 2 * math.pi + elif current_error < -math.pi: + current_error += 2 * math.pi + + # 计算已旋转角度（用于检测机器人是否在移动） + rotated = current_yaw_now - last_yaw + if abs(rotated) > math.pi: + rotated = 2 * math.pi - abs(rotated) + rotated *= -1 if current_yaw_now > last_yaw else 1 + + last_yaw = current_yaw_now - # 精细调整（如果误差大于限制） - if abs(remaining_dist) > limit: - warning(f"剩余误差: {math.degrees(remaining_dist):.2f}°, 需要进行微调", "角度") + # 每0.5秒打印一次进度 + elapsed = time.time() - start_time + if int(elapsed * 2) % 10 == 0: # 每5秒 + debug(f"旋转进度: {elapsed:.1f}s, 剩余角度: {math.degrees(abs(current_error)):.2f}°", "进度") - # 进行微调 - const_int_tiny = 1200 if not precision else 1000 # 精确模式下使用更小的系数 - - # 精确模式下使用更小的微调速度 - fine_turn_speed = 0.3 if precision else 0.5 - - # 如果剩余误差很小，进一步降低速度 - if abs(remaining_dist) < 0.1: # 约5.7度 - fine_turn_speed = 0.15 - - # 设置微调命令 + # 判断是否已经接近目标角度 + if abs(current_error) <= limit: + # 立即发送停止命令 msg.mode = 11 msg.gait_id = 26 - msg.vel_des = [0, 0, fine_turn_speed if remaining_dist > 0 else -fine_turn_speed] - - # 精确模式下使用更长的微调时间 - duration_factor = 1.25 if precision else 1.0 - msg.duration = int(const_int_tiny * abs(remaining_dist) * duration_factor) - msg.step_height = [0.06, 0.06] + msg.vel_des = [0, 0, 0] # 停止旋转 + msg.duration = 200 # 短暂停止命令 msg.life_count += 1 - - # 发送命令 ctrl.Send_cmd(msg) - debug(f"发送微调命令：方向={'逆时针' if remaining_dist > 0 else '顺时针'}, 速度={fine_turn_speed}, 持续时间={msg.duration}", "旋转") - # 精确模式下等待更长时间 - wait_time = 5 if not precision else 7 - time.sleep(wait_time) + success(f"旋转成功，误差在允许范围内: {math.degrees(abs(current_error)):.2f}°", "成功") + return True - # 获取最终角度 - final_yaw = ctrl.odo_msg.rpy[2] - info(f"微调完成: 最终角度={math.degrees(final_yaw):.2f}°, 目标角度={math.degrees(target_yaw):.2f}°", "角度") + # 检测是否接近目标角度，需要减速 + if abs(current_error) < 0.3 and abs(turn_speed) > 0.15: # 约17度内减速 + turn_speed = 0.15 * (1 if turn_speed > 0 else -1) + msg.vel_des = [0, 0, turn_speed] + msg.life_count += 1 + ctrl.Send_cmd(msg) + debug(f"接近目标，减速至: {turn_speed}", "减速") - final_error = abs(target_yaw - final_yaw) - if final_error > math.pi: - final_error = 2 * math.pi - final_error + # 如果旋转方向不对（过头或方向错误），修正方向 + if (current_error > 0 and turn_direction < 0) or (current_error < 0 and turn_direction > 0): + turn_direction = 1 if current_error > 0 else -1 + turn_speed = 0.15 # 方向调整时使用低速 + msg.vel_des = [0, 0, turn_speed * turn_direction] + msg.life_count += 1 + ctrl.Send_cmd(msg) + debug(f"调整旋转方向: {'逆时针' if turn_direction > 0 else '顺时针'}, 速度={turn_speed}", "方向") + + # 检查超时 + if elapsed > max_timeout: + # 发送停止命令 + msg.mode = 11 + msg.gait_id = 26 + msg.vel_des = [0, 0, 0] # 停止旋转 + msg.duration = 500 + msg.life_count += 1 + ctrl.Send_cmd(msg) - # 判断是否成功达到目标 - if final_error <= limit: - success(f"旋转成功，误差在允许范围内: {math.degrees(final_error):.2f}°", "成功") - return True + error(f"旋转超时，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "超时") + return False + + # 检测机器人是否卡住（长时间无明显角度变化） + if abs(rotated) < 0.005: # 角度变化很小 + stable_count += 1 + else: + stable_count = 0 + + # 如果连续多次检测到角度几乎不变，可能卡住了 + if stable_count > 30: # 连续30次检测（约1.5秒）角度几乎不变 + # 尝试增加速度或改变方向 + if abs(turn_speed) < 0.5: + turn_speed = 0.5 * turn_direction + msg.vel_des = [0, 0, turn_speed] + msg.life_count += 1 + ctrl.Send_cmd(msg) + debug(f"检测到旋转停滞，增加速度至: {turn_speed}", "调整") + stable_count = 0 else: - warning(f"旋转完成，但误差超出允许范围: {math.degrees(final_error):.2f}° > {math.degrees(limit):.2f}°", "警告") - - # 在精确模式下，如果误差仍然很大，尝试第二次微调 - if precision and final_error > limit * 2: - info("尝试第二次微调", "精确") - - # 计算第二次微调角度 - second_adj = target_yaw - final_yaw - if second_adj > math.pi: - second_adj -= 2 * math.pi - elif second_adj < -math.pi: - second_adj += 2 * math.pi - - # 降低微调速度 - very_fine_speed = 0.1 - - # 设置第二次微调命令 - msg.mode = 11 - msg.gait_id = 26 - msg.vel_des = [0, 0, very_fine_speed if second_adj > 0 else -very_fine_speed] - msg.duration = int(800 * abs(second_adj)) # 使用更小的系数 - msg.step_height = [0.06, 0.06] - msg.life_count += 1 - - # 发送命令 - ctrl.Send_cmd(msg) - debug(f"第二次微调：旋转{math.degrees(second_adj):.2f}°", "精确") - - # 等待第二次微调完成 - time.sleep(3) - - # 获取最终结果 - final_final_yaw = ctrl.odo_msg.rpy[2] - - final_final_error = abs(target_yaw - final_final_yaw) - if final_final_error > math.pi: - final_final_error = 2 * math.pi - final_final_error - - if final_final_error <= limit: - success(f"第二次微调成功，最终误差: {math.degrees(final_final_error):.2f}°", "成功") - return True - else: - warning(f"两次微调后仍有误差: {math.degrees(final_final_error):.2f}°", "警告") - # 如果误差已经比第一次小，算作改进 - return final_final_error < final_error - + # 如果速度已经很大还是卡住，可能有障碍，停止旋转 + msg.vel_des = [0, 0, 0] # 停止旋转 + msg.life_count += 1 + ctrl.Send_cmd(msg) + error(f"检测到旋转异常，已停止。当前误差: {math.degrees(abs(current_error)):.2f}°", "异常") return False - - success("旋转成功完成", "完成") - return True