diff --git a/test.py b/test.py new file mode 100644 index 0000000..0f3b030 --- /dev/null +++ b/test.py @@ -0,0 +1,129 @@ +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) \ No newline at end of file