新增go_to_x_v2函数以支持机器人移动到指定x坐标，更新task_3.py以引入新函数并调整稳定性检测参数，优化代码逻辑和文件处理。

base_move/go_to_xy.py

@@ -637,3 +637,16 @@ def go_to_y_v2(ctrl, msg, target_y, speed=0.5, precision=True, observe=False):
     """
     target_x = ctrl.odo_msg.xyz[0]
     return go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)
+
+def go_to_x_v2(ctrl, msg, target_x, speed=0.5, precision=True, observe=False):
+    """
+    控制机器人移动到指定的x坐标位置，使用直接x速度控制
+    
+    参数:
+    ctrl: Robot_Ctrl 对象，包含里程计信息
+    msg: robot_control_cmd_lcmt 对象，用于发送命令
+    target_y: 目标Y坐标(米)
+    speed: 行走速度(米/秒)，范围0.1~1.0，默认为0.5
+    """
+    target_y = ctrl.odo_msg.xyz[1]
+    return go_to_xy_v2(ctrl, msg, target_x, target_y, speed, precision, observe)

task_3/task_3.py

@@ -14,6 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)))
 from utils.log_helper import LogHelper, get_logger, section, info, debug, warning, error, success, timing
 from base_move.turn_degree import turn_degree, turn_degree_v2
 from base_move.go_straight import go_straight
+from base_move.go_to_xy import go_to_x_v2
 from file_send_lcmt import file_send_lcmt
 
 # 创建本模块特定的日志记录器
@@ -96,18 +97,14 @@ def run_task_3(ctrl, msg):
         
         # 参数设置
         stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 10  # 连续15次检测z轴不再增加则认为已经停止
+        stable_threshold = 8  # 连续15次检测z轴不再增加则认为已经停止
         z_speed_threshold = 0.01  # z轴速度阈值，小于这个值认为已经停止爬升
         climb_speed_threshold = 0.05  # 检测到开始爬坡的速度阈值
-        max_iterations = 600  # 最大循环次数，作为安全保障
+        max_iterations = 250  # 最大循环次数，作为安全保障
-        min_iterations = 200  # 最小循环次数，作为安全保障
+        min_iterations = 150  # 最小循环次数，作为安全保障
-        start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
 
         # 阶段控制
         climbing_detected = False  # 是否检测到正在爬坡
-        
-        info(f"开始监测里程计Z轴速度，初始高度: {start_height}", "监测")
-        
         for i in range(max_iterations):
             # 发送控制命令维持心跳
             ctrl.Send_cmd(msg)
@@ -129,11 +126,10 @@ def run_task_3(ctrl, msg):
             # 只有在检测到爬坡后，才开始监控Z轴是否停止增加
             if i > min_iterations and climbing_detected:
                 # 如果Z轴速度接近于0或者为负，表示已经停止爬升或开始下降
-                if abs(vz) < z_speed_threshold or vz < 0:
+                if abs(vz) < z_speed_threshold: #  or vz < 0:
                     stable_count += 1
                     if stable_count >= stable_threshold:
-                        current_height = ctrl.odo_msg.xyz[2]
+                        info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}", "监测")
-                        info(f"Z轴速度趋近于0，停止循环。当前速度: {vz:.3f}, 当前高度: {current_height:.3f}", "监测")
                         break
                 else:
                     # 如果Z轴仍有明显上升速度，重置稳定计数
@@ -148,21 +144,20 @@ def run_task_3(ctrl, msg):
         ctrl.Send_cmd(msg)
         pass
 
-    section('任务3-2：直线行走', "开始")
+    section('任务3-2：x = 2', "开始")
-    msg.mode = 11  # Locomotion模式
+    # msg.mode = 11  # Locomotion模式 # DEBUG
-    msg.gait_id = 26  # 自变频步态
+    # msg.gait_id = 26  # 自变频步态
-    msg.duration = 0  # wait next cmd
+    # msg.duration = 0  # wait next cmd
-    msg.step_height = [0.06, 0.06]  # 抬腿高度
+    # msg.step_height = [0.06, 0.06]  # 抬腿高度
-    msg.vel_des = [0, 0.2, 0]  # [前进速度, 侧向速度, 角速度]
+    # msg.vel_des = [0, 0.2, 0]  # [前进速度, 侧向速度, 角速度]
-    msg.life_count += 1
+    # msg.life_count += 1
-    ctrl.Send_cmd(msg)
+    # ctrl.Send_cmd(msg)
-    time.sleep(0.3)
+    time.sleep(1)
-
+    # go_to_x_v2(ctrl, msg, 2, speed=0.5, precision=True, observe=True)
-    return
 
     section('任务3-3：down', "完成")
     try:
-        steps = toml.load("./task_3/Gait_Params_up.toml")
+        steps = toml.load("./task_3/Gait_Params_down.toml")
         full_steps = {'step':[robot_cmd]}
         k = 0
         for i in steps['step']:
@@ -189,14 +184,14 @@ def run_task_3(ctrl, msg):
             else:
                 full_steps['step'].append(cmd)
             k=k+1
-        f = open("./task_3/Gait_Params_up_full.toml", 'w')
+        f = open("./task_3/Gait_Params_down_full.toml", 'w')
         f.write("# Gait Params\n")
         f.writelines(toml.dumps(full_steps))
         f.close()
 
         # pre
         file_obj_gait_def = open("./task_3/Gait_Def_up.toml",'r')
-        file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r')
+        file_obj_gait_params = open("./task_3/Gait_Params_down_full.toml",'r')
         usergait_msg.data = file_obj_gait_def.read()
         lcm_usergait.publish("user_gait_file",usergait_msg.encode())
         time.sleep(0.5)
@@ -206,12 +201,6 @@ def run_task_3(ctrl, msg):
         file_obj_gait_def.close()
         file_obj_gait_params.close()
 
-        file_obj_gait_params = open("./task_3/Gait_Params_up_full.toml",'r')
-        usergait_msg.data = file_obj_gait_params.read()
-        lcm_usergait.publish("user_gait_file", usergait_msg.encode())
-        time.sleep(0.5)
-        file_obj_gait_params.close()
-
         msg.mode = 62
         msg.value = 0
         msg.contact = 15
@@ -221,11 +210,11 @@ def run_task_3(ctrl, msg):
         
         # 参数设置
         stable_count = 0  # 用于计数z轴稳定的次数
-        stable_threshold = 8  # 连续10次检测z轴速度接近零则认为已经到达平地
+        stable_threshold = 10  # 连续10次检测z轴速度接近零则认为已经到达平地
-        z_speed_threshold = 0.01  # z轴速度阈值，小于这个值认为已经停止下降
+        z_speed_threshold = 0.005  # z轴速度阈值，小于这个值认为已经停止下降
         descent_speed_threshold = -0.05  # 检测到开始下坡的速度阈值（负值表示下降）
-        max_iterations = 600  # 最大循环次数，作为安全保障
+        max_iterations = 250  # 最大循环次数，作为安全保障
-        min_iterations = 100  # 最小循环次数，确保有足够的时间开始动作
+        min_iterations = 150  # 最小循环次数，确保有足够的时间开始动作
         start_height = ctrl.odo_msg.xyz[2]  # 记录起始高度
         
         # 阶段控制