|
|
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) |
