ADD file via upload

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)