import Unitree_Python_sdk
import time

unitree_robot = Unitree_Python_sdk.Unitree_Robot_High()

# motion_time = 0
# while True:
# 	time.sleep(0.002)
# 	motion_time += 1
# 	if (motion_time < 1000):
# 		unitree_robot.robot_walking(forwardSpeed=0.1, sideSpeed=0.0, rotateSpeed=0.0, bodyHeight=0.0, footRaiseHeight=0.1) #前进
# 	if (motion_time >= 1000 and motion_time < 2000):
# 		unitree_robot.robot_walking(forwardSpeed=-0.1, sideSpeed=0.0, rotateSpeed=0.0, bodyHeight=0.0, footRaiseHeight=0.1) #后退
# 	if (motion_time >= 2000):
# 		unitree_robot.robot_walking(forwardSpeed = 0.0, sideSpeed = 0.0, rotateSpeed = 0.1, bodyHeight = 0.0) #打转

yz = [0.1, 0.1, 0.1]

import serial
import threading
import time
import binascii
import re
from statistics import mean
import numpy as np
import sys
import openpyxl
import pandas as pd

STRGLO = ""  # 读取的数据
BOOL = True  # 读取标志位


# 读数代码本体实现
def ReadData(ser):
    global STRGLO, BOOL
    # 循环接收数据，此为死循环，可用线程实现.有时候硬件接口问题常导致收不到数据要合理处理。并对接收的数据进行一定处理。
    while BOOL:
        if ser.in_waiting:
            # t = time.ctime()  # 获取到数据的一个时间点，其实这里时间和数值的对应关系需要根据相应速度适当调整
            # 获取时方式2，当地时间获取
            timelocal = time.localtime(time.time())
            datetimestr = "%s-%s-%s %s:%s:%s" % (
            timelocal.tm_year, timelocal.tm_mon, timelocal.tm_mday, timelocal.tm_hour, timelocal.tm_min,
            timelocal.tm_sec)
            datastr = "%s-%s-%s" % (timelocal.tm_year, timelocal.tm_mon, timelocal.tm_mday)
            timestr = "%s:%s:%s" % (timelocal.tm_hour, timelocal.tm_min, timelocal.tm_sec)

            STRGLO = ser.read(ser.in_waiting)  # b'\xfa\x19-\xaa' ,b'\xfa\x1a\x16\xaa\xfa\x1a\x16\xaa'  # data在后面进行截取。

            # 判断数据格式并做处理
            hex_list = list(STRGLO)
            data_len = len(hex_list)
            if data_len < 4 or hex_list[0] != 0xfa or hex_list[-1] != 0xaa:
                continue
            else:
                # print(hex_list)
                strength1 = hex_list[3]
                strength2 = hex_list[8]
                strength3 = hex_list[13]
                strength1 = 1.18327 * strength1 + 0.0113 * strength1 * strength1 + 0.0000610045 * strength1 * strength1 * strength1
                strength2 = 1.16311 * strength2 + 0.0108 * strength2 * strength2 + 0.000062639 * strength2 * strength2 * strength2
                strength3 = 0.86501 * strength3 + 0.0166 * strength3 * strength3 + 0.0000425563 * strength3 * strength3 * strength3
                print(strength1, strength2, strength3)
                if strength1 > yz[0]:
                    unitree_robot.robot_walking(forwardSpeed=0.2, sideSpeed=0.0, rotateSpeed=0.0, bodyHeight=0.0,
                                                footRaiseHeight=0.1)
                    print('前进')
                if strength2 > yz[1]:
                    unitree_robot.robot_walking(forwardSpeed=-0.5, sideSpeed=0.0, rotateSpeed=0.0, bodyHeight=0.0,
                                                footRaiseHeight=0.1)
                    print('后退')
                if strength3 > yz[2]:
                    unitree_robot.robot_walking(forwardSpeed=0.0, sideSpeed=0.0, rotateSpeed=0.5, bodyHeight=0.0,
                                                footRaiseHeight=0.1)
                    print('打转')

                # 打开串口


# 端口，GNU / Linux上的/ dev / ttyUSB0 等 或 Windows上的 COM3 等
# 波特率，标准值之一：50,75,110,134,150,200,300,600,1200,1800,2400,4800,9600,19200,38400,57600,115200
# 超时设置,None：永远等待操作，0为立即返回请求结果，其他值为等待超时时间(单位为秒）
def DOpenPort(portx, bps, timeout):
    global ser, ret
    ret = False
    try:
        # 打开串口，并得到串口对象
        ser = serial.Serial(portx, bps, timeout=timeout)
        # 判断是否打开成功,若成功则调用ReadData读取数据获得STRDLO
        if ser.is_open:
            # print("打开成功")
            ret = True
            ser.flushInput()  # 清空缓冲区
            threading.Thread(target=ReadData, args=(ser,)).start()
    # else:
    #     print("open failed")
    #     # DColsePort(ser)  # 关闭端口
    #     ser.close()  # 关闭端口
    except Exception as e:
        print("---异常---：", e)
    return ser, ret


# 关闭串口
def DColsePort(ser):
    global BOOL
    BOOL = False
    threading.currentThread().join()
    ser.close()


# 读数据 传递清空
def DReadPort():
    global STRGLO
    data_byte = STRGLO  # 用中间变量data_byte传数
    STRGLO = b''  # 清空当次读数，并且是字节类型 b''
    return data_byte  # 返回当次读数


# 主程序
if __name__ == "__main__":
    ser, ret = DOpenPort('/dev/ttyUSB0', 115200, None)  # 串口根据自己电脑情况手动输入更新，我这里是COM9，这里已经调用了ReadData（）
    if ret:  # 判断串口是否成功打开，成功打开的话循环读取数据
        # print("打开成功")
        while True:
            ReadData(ser)  # 读数据并处理显示。
    # 关闭串口
    DColsePort(ser)