import time

def run_task_5(ctrl, msg):
    # DEBUG
    # msg.mode = 11  # 运动模式
    # msg.gait_id = 26
    # msg.vel_des = [0, 0, 2]  # 期望速度
    # msg.duration = 0  # 零时长表示持续运动，直到接收到新命令
    # msg.step_height = [0.02, 0.02]  # 持续运动时摆动腿的离地高度
    # msg.life_count += 1
    # ctrl.Send_cmd(msg)
    # time.sleep(1.1)  # 持续5秒钟

    # msg.mode = 11
    # msg.gait_id = 3
    # msg.vel_des = [1, 0, 0]
    # msg.duration = 1000
    # msg.step_height = [0.03, 0.03]
    # msg.life_count += 1
    # print(msg.pos_des)
    # ctrl.Send_cmd(msg)
    # ctrl.Wait_finish(11, msg.gait_id)

    msg.mode = 3
    msg.gait_id = 0
    msg.pos_des = [0, 0, 0.15]
    msg.life_count += 1
    ctrl.Send_cmd(msg)
    ctrl.Wait_finish(3, msg.gait_id)