'''
This demo show the communication interface of MR813 motion control board bactrlsed 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 go_straight_with_enhanced_calibration

# from robot_control_cmd_lcmt import robot_control_cmd_lcmt
# from robot_control_response_lcmt import robot_control_response_lcmt

def circle1(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 = 4600          # 执行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 = 3400
        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)

#上坡
        go_straight_with_enhanced_calibration(ctrl, msg, distance = 5, speed=0.5, observe=False, mode=11, gait_id=3, step_height=[0.21, 0.21])

#停五秒
        msg.mode = 12 
        msg.gait_id = 0
        msg.vel_des = [0.0,0.0,0.0]
        msg.duration = 6000
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(7)

#直走
        msg.mode = 11 
        msg.gait_id = 3
        msg.vel_des = [0.3,0.0,0.0]
        msg.duration = 9000
        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 = 4000          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(5)

#直走进入扫码区
        msg.mode = 11
        msg.gait_id = 3
        msg.vel_des = [0.3,0.0,0.0]
        msg.duration = 3400
        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 = 4600          # 执行9.5秒后自动停止
        msg.step_height = [0.06, 0.06]
        msg.life_count += 1
        ctrl.Send_cmd(msg)
        time.sleep(5)

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