'''
This demo show the communication interface of MR813 motion control board based on Lcm.
Dependency: 
- robot_control_cmd_lcmt.py
- robot_control_response_lcmt.py
'''
import lcm
import sys
import os
import time
import threading
from threading import Thread, Lock
from task_3.task_3 import pass_stone

# from robot_control_cmd_lcmt import robot_control_cmd_lcmt
# from robot_control_response_lcmt import robot_control_response_lcmt

def circle2(ctrl, msg):
    try:
#起立
        msg.mode = 12 
        msg.gait_id = 0
        msg.life_count += 1 # Command will take effect when life_count update
        ctrl.Send_cmd(msg)
        ctrl.Wait_finish(12, 0)

# #向左转
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [0.0, 0.0, 0.4]   # 顺时针旋转（负Z轴速度）
        msg.duration = 4200          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(10)



#直走 
        msg.mode = 11
        msg.gait_id = 3
        msg.vel_des = [0.3,0.0,0.0]
        msg.duration = 2800
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(10)

# #向右转
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [0.0, 0.0, -0.4]   # 顺时针旋转（负Z轴速度）
        msg.duration = 4200          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(10)


# 过石板路
        pass_stone(ctrl, msg, distance = 4.5, observe=False)

# 旋转至正面
        msg.mode = 11                # 旋转1
        msg.gait_id = 3             # 自变频Trot步态
        msg.vel_des = [0.0, 0.0, 0.4]   # 顺时针旋转（负Z轴速度）
        msg.duration = 8400          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(10)

    except KeyboardInterrupt:
        pass
    ctrl.quit()
    sys.exit()