搜索到
8
篇与
测试
的结果
-
-
航迹推算张 import serial import time import math def get_date(): """ 此函数通过串口接收数据，并把每一帧数据提取出来，以列表的形式存储每一帧数据，为后续数据提取做基础 :return: a_list 每一帧数据的列表 """ ser = serial.Serial( # 下面这些参数根据情况修改 port='COM5', # 串口 baudrate=115200, # 波特率 parity=serial.PARITY_NONE, # 校验位 stopbits=serial.STOPBITS_ONE, # 停止位 bytesize=serial.EIGHTBITS, # 字节大小 timeout=1) while True: msg = ser.read_all() # 读取全部数据 # msg = ser.readline() # 读取遇换行符换行 # msg = b'\x00\x00\x00\x00\x00\x00\x00\xfb\xa6\xfe46(\x00\x00\x00\x01\x00\x01_\xe8E' 原始数据格式，后续要做转换 # print('raw', msg) """ 备用代码 d = 1 for key in msg: if d % 24 == 0: print(hex(key)) else: print(hex(key), end=' ') d = d + 1 """ a_list = [] # 空列表 for key in msg: a = hex(key) # 将stm32发来的原始数据转换为16进制 # 提取包含帧头帧尾24字节的16进制数据并保存到a_list列表中 while a == "0x7b" and len(a_list) < 25: for key01 in msg: a = hex(key01) a_list.append(a) if len(a_list) == 24 and a_list[2] != "0x7b": return a_list pos_x = 0 # 此处赋予小车初始位姿 接收二维码赋予的初值 pos_y = 0 theta = 0 sampling_time = 0.02 # 积分时间 while True: b_list = get_date() # 测试代码 # print(b_list[2], b_list[3]) # print(int(b_list[2], 16), int(b_list[3], 16), int(b_list[19], 16)) # print(type(int(b_list[3], 16))) # print("小车速度：%f 小车角速度：%f" % ((float(int(b_list[3], 16))) / 1000, (float(int(b_list[19], 16))) / 3753)) # 此处用来判断小车前进或者后退，逆时针旋转或者顺时针旋转。然后在此基础上赋予速度变量和角速度变量正负号 # print(type(bin(int(b_list[2], 16))[2])) 注意此处为字符串类型，需要做转换才能判断 【易搞错，重要！】 if int(bin(int(b_list[2], 16))[2]) == 0: # 判断高八位中的最高位二进制是否为0，若为0，则小车前进,在这里进行了二进制转换 robot_vel = (float(int(b_list[2] + b_list[3][2:], 16))) / 1000 else: robot_vel = -((int("0xffff", 16) - int(b_list[2] + b_list[3][2:], 16)) / 1000) if int(bin(int(b_list[18], 16))[2]) == 0: # 判断高八位中的最高位二进制是否为0，若为0，则小车绕Z轴逆时针旋转正方向，在这里进行了二进制转换 angular_velocity_z = (float(int(b_list[18] + b_list[19][2:], 16))) / 3753 else: angular_velocity_z = -((int("0xffff", 16) - int(b_list[18] + b_list[19][2:], 16)) / 3753) # robot_vel = (float(int(b_list[3], 16))) / 1000 此为未加判断速度和角速度方向时的旧代码 # angular_velocity_z = (float(int(b_list[19], 16))) / 3753 if robot_vel != float(0): # 有编码器的速度则进行计算 if angular_velocity_z < float(0.03): # imu静止时也会有微小输出，设置一个阈值来表示小车实际运动了 angular_velocity_z = float(0) theta += angular_velocity_z * sampling_time pos_x += (robot_vel * math.cos(theta)) * sampling_time pos_y += (robot_vel * math.sin(theta)) * sampling_time else: theta += angular_velocity_z * sampling_time pos_x += (robot_vel * math.cos(theta)) * sampling_time pos_y += (robot_vel * math.sin(theta)) * sampling_time else: angular_velocity_z = 0 # 若小车速度为0,此处应赋值等于初始位姿，即位姿不发生变化 print("小车速度是:%f 航向数据是:%f x轴数据是:%f y轴的数据是:%f" % (robot_vel, theta, pos_x, pos_y))
-
-
-
