Havoc420mac/mi-task
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Merge branch 'fix-arc' into main

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-18 07:58:30 +00:00
commit f4eab6b296
4 changed files with 72 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
base_move/go_straight.py
View File

@ -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__":

94
base_move/move_base_hori_line.py
View File

@ -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:

3
main.py
View File

@ -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()

19
task_1/task_1.py
View File

@ -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)