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

🎏 Adjust movement parameters in task 2 and task 2.5 for improved execution, and enhance monitoring in task 3 by adding target y-coordinate checks and refining output messages.

Browse Source
This commit is contained in:
havoc420ubuntu 2025-05-31 22:01:48 +00:00
parent 56c4131fa4
commit 0bb157240e
3 changed files with 26 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
task_2/task_2.py
View File

@ -293,7 +293,7 @@ def run_task_2(ctrl, msg, xy_flag=False):
return arrow_direction return arrow_direction
def run_task_2_back(ctrl, msg): def run_task_2_back(ctrl, msg):
go_to_xy(ctrl, msg, 0.7, 5, speed=0.5, observe=True) go_to_xy(ctrl, msg, 0.7, 4.8, speed=0.5, observe=True)
turn_degree_v2(ctrl, msg, -106.5, absolute=True) turn_degree_v2(ctrl, msg, -106.5, absolute=True)
print('角度为',ctrl.odo_msg.rpy[2]) print('角度为',ctrl.odo_msg.rpy[2])

4
task_2_5/task_2_5.py
View File

@ -28,7 +28,7 @@ def run_task_2_5(Ctrl, msg, direction='left'):
target_distance=0.2, target_distance=0.2,
detect_func_version=3, detect_func_version=3,
pass_align=True, pass_align=True,
radius=0.55, radius=0.6,
observe=observe, observe=observe,
no_end_reset=True, no_end_reset=True,
) )
@ -40,4 +40,4 @@ def run_task_2_5_back(Ctrl, msg, direction='left'):
section('任务2.5-back：预备进入任务3', "启动") section('任务2.5-back：预备进入任务3', "启动")
turn_degree_v2(Ctrl, msg, degree=0, absolute=True) turn_degree_v2(Ctrl, msg, degree=0, absolute=True)
go_to_x_v2(Ctrl, msg, target_x=1, observe=observe) go_to_x_v2(Ctrl, msg, target_x=1, observe=observe)
go_to_y_v2(Ctrl, msg, target_y=5.5, speed=0.5, observe=True) go_to_y_v2(Ctrl, msg, target_y=5.2, speed=0.5, observe=True)

76
task_3/task_3.py
View File

@ -101,8 +101,9 @@ def pass_up_down(ctrl, msg):
stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止 stable_threshold = 8 # 连续15次检测z轴不再增加则认为已经停止
z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升 z_speed_threshold = 0.01 # z轴速度阈值，小于这个值认为已经停止爬升
climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值 climb_speed_threshold = 0.05 # 检测到开始爬坡的速度阈值
max_iterations = 230 # 最大循环次数，作为安全保障 max_iterations = 280 # 最大循环次数，作为安全保障
min_iterations = 170 # 最小循环次数，作为安全保障 min_iterations = 170 # 最小循环次数，作为安全保障
y_target = 7.5 # 目标y坐标，达到此值时停止
# 姿态判断参数 # 姿态判断参数
pitch_threshold = 0.05 # 俯仰角阈值（弧度） pitch_threshold = 0.05 # 俯仰角阈值（弧度）
@ -118,7 +119,8 @@ def pass_up_down(ctrl, msg):
# 记录起始姿态和高度 # 记录起始姿态和高度
start_height = ctrl.odo_msg.xyz[2] start_height = ctrl.odo_msg.xyz[2]
info(f"开始监测爬坡过程，初始高度: {start_height:.3f}", "监测") start_y = ctrl.odo_msg.xyz[1]
info(f"开始监测爬坡过程，初始高度: {start_height:.3f}, 初始Y坐标: {start_y:.3f}", "监测")
for i in range(max_iterations): for i in range(max_iterations):
# 发送控制命令维持心跳 # 发送控制命令维持心跳
@ -127,6 +129,7 @@ def pass_up_down(ctrl, msg):
# 获取当前状态数据 # 获取当前状态数据
vz = ctrl.odo_msg.vxyz[2] # Z轴速度 vz = ctrl.odo_msg.vxyz[2] # Z轴速度
current_height = ctrl.odo_msg.xyz[2] # 当前高度 current_height = ctrl.odo_msg.xyz[2] # 当前高度
current_y = ctrl.odo_msg.xyz[1] # 当前y坐标
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度 pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度 vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
@ -140,38 +143,17 @@ def pass_up_down(ctrl, msg):
# 每10次迭代打印一次当前信息 # 每10次迭代打印一次当前信息
if i % 10 == 0: if i % 10 == 0:
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测") info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
# 检测是否开始爬坡阶段 # 检测是否开始爬坡阶段
if not climbing_detected and vz > climb_speed_threshold: if not climbing_detected and vz > climb_speed_threshold:
climbing_detected = True climbing_detected = True
info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测") info(f"检测到开始爬坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
# 多条件判断是否完成爬坡 # 检查是否达到目标y坐标
if i > min_iterations and climbing_detected and len(height_window) == window_size: if current_y >= y_target:
# 计算高度和俯仰角的稳定性 info(f"已达到目标Y坐标: {current_y:.3f}，停止爬坡", "监测")
height_std = np.std(height_window) # 高度标准差
pitch_std = np.std(pitch_window) # 俯仰角标准差
# 多条件综合判断
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
height_stable = height_std < 0.01 # 高度变化小
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
# 综合判断条件
if (position_stable and attitude_stable and angular_rate_stable) or \
(position_stable and height_stable and pitch_stable) or \
(vbody_stable and attitude_stable and height_stable):
stable_count += 1
if stable_count >= stable_threshold:
info(f"检测到已完成爬坡：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 当前高度: {current_height:.3f}\n - 上升了: {current_height - start_height:.3f}米", "监测")
break break
else:
# 重置稳定计数
stable_count = 0
time.sleep(0.2) time.sleep(0.2)
except KeyboardInterrupt: except KeyboardInterrupt:
@ -185,7 +167,7 @@ def pass_up_down(ctrl, msg):
section('任务3-2：x = 2', "开始") section('任务3-2：x = 2', "开始")
turn_degree_v2(ctrl, msg, 90, absolute=True) turn_degree_v2(ctrl, msg, 90, absolute=True)
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False, detect_height=0.3)
go_straight(ctrl, msg, distance=0.5, speed=0.5, observe=True) go_straight(ctrl, msg, distance=0.3, speed=0.5, observe=True)
time.sleep(1) time.sleep(1)
section('任务3-3：down', "完成") section('任务3-3：down', "完成")
@ -249,6 +231,8 @@ def pass_up_down(ctrl, msg):
max_iterations = 110 # 最大循环次数，作为安全保障 max_iterations = 110 # 最大循环次数，作为安全保障
min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作 min_iterations = 80 # 最小循环次数，确保有足够的时间开始动作
start_height = ctrl.odo_msg.xyz[2] # 记录起始高度 start_height = ctrl.odo_msg.xyz[2] # 记录起始高度
start_y = ctrl.odo_msg.xyz[1] # 记录起始y坐标
y_target = 10.3 # 目标y坐标，达到此值时停止
# 姿态判断参数 # 姿态判断参数
pitch_threshold = 0.05 # 俯仰角阈值（弧度） pitch_threshold = 0.05 # 俯仰角阈值（弧度）
@ -263,7 +247,7 @@ def pass_up_down(ctrl, msg):
pitch_window = [] pitch_window = []
window_size = 8 window_size = 8
info(f"开始监测下坡过程，初始高度: {start_height}", "监测") info(f"开始监测下坡过程，初始高度: {start_height}, 初始Y坐标: {start_y:.3f}", "监测")
for i in range(max_iterations): for i in range(max_iterations):
# 发送控制命令维持心跳 # 发送控制命令维持心跳
@ -272,6 +256,7 @@ def pass_up_down(ctrl, msg):
# 获取当前状态数据 # 获取当前状态数据
vz = ctrl.odo_msg.vxyz[2] # Z轴速度 vz = ctrl.odo_msg.vxyz[2] # Z轴速度
current_height = ctrl.odo_msg.xyz[2] # 当前高度 current_height = ctrl.odo_msg.xyz[2] # 当前高度
current_y = ctrl.odo_msg.xyz[1] # 当前y坐标
current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角 current_pitch = ctrl.odo_msg.rpy[1] # 当前俯仰角
pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度 pitch_rate = ctrl.odo_msg.omegaBody[1] # 俯仰角速度
vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度 vbody_z = ctrl.odo_msg.vBody[2] # 机体坐标系Z速度
@ -285,44 +270,23 @@ def pass_up_down(ctrl, msg):
# 每10次迭代打印一次当前信息 # 每10次迭代打印一次当前信息
if observe and i % 10 == 0: if observe and i % 10 == 0:
info(f"Step:{i} 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}, 角速度={pitch_rate:.3f}", "监测") info(f"Step:{i} 当前Y坐标={current_y:.3f}, 当前Z轴速度={vz:.3f}, 当前高度={current_height:.3f}, 俯仰角={current_pitch:.3f}", "监测")
# 检测是否开始下坡阶段 # 检测是否开始下坡阶段
if not descending_detected and vz < descent_speed_threshold: if not descending_detected and vz < descent_speed_threshold:
descending_detected = True descending_detected = True
info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测") info(f"检测到开始下坡，Z轴速度: {vz:.3f}, 当前高度: {current_height:.3f}, 俯仰角: {current_pitch:.3f}", "监测")
# 多条件判断是否到达平地 # 检查是否达到目标y坐标
if i > min_iterations and descending_detected and len(height_window) == window_size: if current_y >= y_target:
# 计算高度和俯仰角的稳定性 info(f"已达到目标Y坐标: {current_y:.3f}，停止下坡", "监测")
height_std = np.std(height_window) # 高度标准差
pitch_std = np.std(pitch_window) # 俯仰角标准差
# 多条件综合判断
position_stable = abs(vz) < z_speed_threshold # 垂直速度稳定
attitude_stable = abs(current_pitch) < pitch_threshold # 俯仰角接近水平
angular_rate_stable = abs(pitch_rate) < angular_rate_threshold # 角速度稳定
height_stable = height_std < 0.01 # 高度变化小
pitch_stable = pitch_std < 0.01 # 俯仰角变化小
vbody_stable = abs(vbody_z) < 0.01 # 机体Z方向速度稳定
# 综合判断条件
if (position_stable and attitude_stable and angular_rate_stable) or \
(position_stable and height_stable and pitch_stable) or \
(vbody_stable and attitude_stable and height_stable):
stable_count += 1
if stable_count >= stable_threshold:
info(f"检测到已到达平地：\n - Z轴速度: {vz:.3f}\n - 俯仰角: {current_pitch:.3f}\n - 角速度: {pitch_rate:.3f}\n - 高度: {current_height:.3f}\n - 下降了: {start_height - current_height:.3f}米", "监测")
flat_ground_detected = True flat_ground_detected = True
break break
else:
# 重置稳定计数
stable_count = 0
time.sleep(0.2) time.sleep(0.2)
if not flat_ground_detected: if not flat_ground_detected:
info(f"达到最大循环次数，未能明确检测到到达平地。当前高度: {ctrl.odo_msg.xyz[2]:.3f}", "警告") info(f"达到最大循环次数，未能到达目标Y坐标。当前Y坐标: {ctrl.odo_msg.xyz[1]:.3f}", "警告")
except KeyboardInterrupt: except KeyboardInterrupt:
msg.mode = 7 #PureDamper before KeyboardInterrupt: msg.mode = 7 #PureDamper before KeyboardInterrupt:
msg.gait_id = 0 msg.gait_id = 0
@ -503,7 +467,7 @@ def run_task_3(ctrl, msg, time_sleep=5000):
center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False) center_on_dual_tracks(ctrl, msg, max_deviation=10.0, observe=False)
section('任务3-2：yellow stop', "开始") section('任务3-2：yellow stop', "开始")
# go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3) go_until_yellow_area(ctrl, msg, yellow_ratio_threshold=0.15, speed=0.3)
# 原地站立3秒 # 原地站立3秒
section("原地站立3秒", "站立") section("原地站立3秒", "站立")