树莓派控制智能体方案设计
**
1.树莓派作为上位机，STM32板子作为下位机
方法思路：树莓派上位机上用python跑算法，将结算的控制量（如航向角、速度等）通过串口发送给下位机，并实时接收下位机的相关数据（坐标、航向角、速度等）进行外环闭环；下位机作为内环，进行PWM控制（可选择PID控制产生相应的PWM）
上位机与下位机通信代码（串口收发如下）：项目全部代码可联系学习
import numpy as np
import serial
from math import radians, sin, cos, asin, sqrt,atan
import math
import matplotlib.pyplot as plt
import csv
import threading
import time
from multiprocessing import Process, Queue
import keyboard, csv, serial, os
class Serial_Decode(object): ##函数注释：串口接收类
"""
初始化参数：
bufsize：缓存区长度，可根据需要解析的包大小调节
com:串口号，是字符串，例如'COM9'
band：波特率，
debug：是否输出调试信息
功能：
实现对串口协议的解析，协议内容：
帧头：
0xAA 1个字节
功能字：
0xE2, 一个字节
数据长度：
0x0A, 一个字节，代表数据内容长度，必定为偶数
数据内容：
字节个数不确定，每两个字节组成一个16位无符号整形数
高位在前，地位在后
校验和：
一个字节，为前面所以字节的和取低八位
"""
def __init__(self, bufsize=200, com='COM1', band=9600, debug=False):
"""
输入参数：
bufsize：缓存区长度，可根据需要解析的包大小调节
com:串口号，是字符串，例如'COM9'
band：波特率，
debug：是否输出调试信息
"""
self.current = 0 # 当前buf中数据长度
self.bufsize = bufsize
self.buf = [0 for i in range(bufsize)] # 保存数据buf,必须按照长度初始化
self.data_signal = [] # 解析到的有效数据
self.data_para = [] #功能字 源地址 目的地址
self.debug = debug # 是否输出调试信息
self.newdata = False # 自上次取获取数据后是否解析出新的的数据
self.ser = serial.Serial(com, band)
def getdata(self): # 获取解析到的数据
serialbuf = self.ser.read_all()
self.append(serialbuf)
newdata = self.newdata
self.newdata = False
return newdata, self.data_para, self.data_signal
def append(self, serialbuf): # 把串口数据加入到buf中，加入完成后尝试进行解析
filebuf = np.frombuffer(serialbuf, dtype=np.uint8) ##更改 fromstring.
if self.current + filebuf.size > self.bufsize:
self.buf[self.current:self.bufsize] = \
filebuf[0:self.bufsize - self.current - filebuf.size]
else:
self.buf[self.current:self.current + filebuf.size] = filebuf
self.current += filebuf.size
self.process()
def process(self): # 进行数据解析
while self.current > 71: # 如果队列中数据多于2包数据 71
if self.buf[0] == 170 and self.buf[2] ==175 : #170就是0xAA ； 0xAF= 175
if self.debug:
print("检测到帧头，功能字是" + str(self.buf[3]))
datalength = self.buf[4] # 有效数据长度
framelength = datalength + 6 # 帧长度
datasum = np.sum(self.buf[0:framelength - 1]) % 256
if datasum == self.buf[framelength - 1]: # 校验通过
self.data_para = self.buf[1:4] # 源地址 目的地址 功能字
self.data_signal = self.buf[5:5 + datalength] ### [5:5 + datalength] 不包括 5 + datalength
# self.data_signal = np.array(
# self.data_signal, dtype='int8') ### 有符号
self.data_signal = np.array(self.data_signal)
self.data_signal = np.concatenate(
(self.data_signal[0:2].astype('uint8'), self.data_signal[2:datalength].astype('int16'))) ## yaw、ratayaw、vel有符号；坐标无符号 ### np.concatenate 用于将两个数组连接在一起
self.data_signal = self.data_signal.reshape(-1, 2)
self.data_signal = self.data_signal[:, 0] * 256 + \
self.data_signal[:, 1] # 第一列元素分别与第二列相加 ： 高位和低位数据合并成完整的数值
self.newdata = True
if self.debug:
print(self.data_signal)
self.buf = np.roll(self.buf, -framelength)
self.current -= framelength
if self.debug:
print("解析到一帧数据")
else: # 校验失败
if self.debug:
print("校验和错误")
if 170 in self.buf[2:self.current]: # 帧头对，但是校验和错误
temparray = self.buf[2:self.current]
if not isinstance(temparray, list):
temparray = temparray.tolist()
offset = temparray.index(170)
self.buf = np.roll(self.buf, -offset)
self.current -= offset
### 如果解析不到，舍弃前面的数据，直到data[2] == 173 表示上位机是树莓派
elif 170 in self.buf[0:self.current]: ##170
if self.debug:
print("接收到无效数据")
temparray = self.buf[0:self.current]
if not isinstance(temparray, list):
temparray = temparray.tolist()
offset = temparray.index(170) # 查找为170的索引赋值=offset 170
self.buf = np.roll(self.buf, -offset) #向-左滚动offset个位数，移除前面的无效数据
self.current -= offset
###########发送数据############
def send_data(self, data, para):
'''
数据发送到下位机：
A.协议：0xAA + para + bytes([lengh] + data + check
'''
lengh=len(data)
frame = bytes([0xAA]) + para +bytes([lengh])+ data
# 计算校验和
checksum =sum(frame) % 256
senddata = bytes()
check = bytes([checksum])
senddata = frame + check
# 发送数据
self.ser.write(senddata)
if self.debug:
print("开始发送！")
**
2.树莓派作为控制部分（智能体上只需要一个树莓派）
方案：将所有传感器，如IMU、GPS、激光雷达等传感器直接通过usb接到树莓派上，树莓派进行接收处理相应数据；并进行算法的结算，输出相应控制量直接生成PWM，控制智能体航行。
参考代码如下（简单例子：一个传感器、PWM）
'''
ser.read(num) # 读取收到的num个字节的数据
ser.inWaiting() # 可以获取还未读出的数据
'''
import serial
import struct
import wiringpi as wp
import time
a=1 #wuli_pin12 left
b=23 #wuli_pin33 right
wp.wiringPiSetup() #使用bcm编码方式
wp.pinMode(a,2) #使用pin18 为pwm输出
wp.pinMode(b,2)
wp.pwmSetMode(0)
#wp.digitalWrite(1,1) #pin18口输出高电平
wp.pwmSetRange(1024)
wp.pwmSetClock(380) #设置分频系数 50hz
#W=20
#W=input("please input dc: ")
#for bright in range (10,1024,20):
bright=76#mid value
wp.pwmWrite(a,bright)#int(W))
wp.pwmWrite(b,bright)
time.sleep(2)
bright_a=84#mid value
bright_b=68#mid value
wp.pwmWrite(a,bright_a)#int(W))
wp.pwmWrite(b,bright_b)
ser = serial.Serial("/dev/ttyUSB0", 9600)
ser.flushInput() # 清除缓存
desire_course=110
while True:
count = ser.inWaiting() # 获取还有多少字符未读
#print(count)
if count != 0:
byte_string = ser.read(count)#.decode(encoding='gb18030') # 读取数据存到data中
hex_representation = byte_string.hex()
# print(hex_representation[int(22):int(24)])
# print(hex_representation)
if hex_representation[22:24] =='55':
#print(hex_representation[22:44])
#print(hex_representation[44:66])
fp1=open('Yaw_84_68.txt','a')
fp2=open('Wz_84_68.txt','a')
course_v=hex_representation[22:44]
course=hex_representation[44:66]
#print(int(course[4:6],16))
#print(int(course[6:8],16))
#print(course[8:10])
Roll=(int(course[4:6],16)+int(course[6:8],16)*16*16)/(32768/180)
Pitch=(int(course[8:10],16)+int(course[10:12],16)*(16*16))/(32768/180)
Yaw=(int(course[12:14],16)+int(course[14:16],16)*(16*16))/(32768/180)
if Yaw>180:
Yaw=Yaw-360
Yaw_d=desire_course-Yaw
W_z=(int(course_v[12:14],16)+int(course_v[14:16],16)*(16*16))/(32768/2000)
fp1.write(str(Yaw))
fp1.write("\n")
fp2.write(str(W_z))
fp2.write("\n")
#print(Roll)
#print(Pitch)
print(Yaw)
#print(Yaw_d)
print(W_z)
#motor_l=int(86-Yaw_d*0.08)
#motor_r=int(86+Yaw_d*0.08)
#print(motor_l)
#print(motor_r)
#wp.pwmWrite(a,motor_l)#int(W))
#wp.pwmWrite(b,motor_r)
fp1.close()
fp2.close()
#print(data) # 打印接受到的数据
time.sleep(0.1) # 系统等待