ros开发增加clion常用模板及初始化配置(三)

我会带着你远行 2022-12-02 00:59 185阅读 0赞

#                       ros开发增加clion常用模板及初始化配置(三) #

python模板

py\_math弧度转角度

import math
    DE2R = math.pi/180
    #弧度=角度*DE2R

py\_unpack通信大端字节转换\_小端

def get_header(self, data):
            return struct.unpack("!iB", data[0:5])
        psize, ptype = self.get_header(data)

py\_socket通讯收发数据

import socket
    import math
    
    DE2R = math.pi/180
    
    # 创建socket连接
    conn = socket.create_connection(('192.168.36.28',30002))
    # 发送指令
    msg = 'movej([{},{},{},{},{},{}], a=1.4, v=1.05, t=0, r=0)\n'.format(-144.98 * DE2R, -97.67 * DE2R, -102.98 * DE2R,-68.95 * DE2R, 83.07 * DE2R, 58.15 * DE2R)
    # conn.send(msg.encode())
    
    movelMsg = 'movel(p[{},{},{},{},{},{}], a=1.2, v=0.25, t=0, r=0)\n'.format(-54.16/1000,-324.52/1000,183.76/1000,3.1225,0.5556,0.2693)
    conn.send(movelMsg.encode())

py\_filter过滤器

pressPoints = list(filter(lambda ele:ele['type']==Point.PRESS,self.paintWidget.point.data))

py\_qt\_btn按钮点击事件

# 绘制点击事件
            paintBtn.clicked.connect(self.paint)

py\_opencv\_waitKey\_VideoCapture按空格保存图片

video = cv.VideoCapture(0)
        # 判断视频是否打开成功
        isOpened = video.isOpened()
        print("视频是否打开成功：",isOpened)
        # 获取图片的信息:帧率
        fps = video.get(cv.CAP_PROP_FPS)
        # 获取每帧宽度
        width = video.get(cv.CAP_PROP_FRAME_WIDTH)
        # 获取每帧的高度
        height = video.get(cv.CAP_PROP_FRAME_HEIGHT)
        print("帧率：{},宽度：{},高度：{}".format(fps,width,height))
    
        # 从视频中读取8帧信息
        count=0
    
        while True:
            count = count + 1
            # 读取成功or失败， 当前帧数据
            flag,frame = video.read()
            #frame=cv.flip(frame, 1) #镜像
            cv.imshow("frame1", frame);#显示视频
            action =cv.waitKey(10) & 0xFF
            ACTION_ESC = 27;
            ACTION_SPACE = 32;
    
            if (action == ACTION_SPACE or action == 'q'):
                aaa(frame )
                return -1
            if (action == ACTION_ESC ):
                return -1
    
            # 将图片信息写入到文件中
            if flag:                                     # 保存
                # 图片的质量
                #cv.imwrite("img/tiger%d.jpg"%count,frame,[cv.IMWRITE_JPEG_QUALITY,100])
                pass
        print("图片截取完成啦！")
        #cv.waitKey(0)
        cv.destroyAllWindows()

py\_opencv\_HSV\_Hsv\_change调试类

import cv2 as cv
    class Hsv_change:
        #hsv 调试
        def __init__(self):
            self.i1 = 0
            self.i2 = 0
            self.i3 = 0
            self.i4 = 0
            self.i5 = 0
            self.i6 = 0
            pass
    
        def onChange(self,a, b, hsv):
            if a == 1:
                self.i1 = b
            elif a == 2:
                self.i2 = b
            elif a == 3:
                self.i3 = b
            elif a == 4:
                self.i4 = b
            elif a == 5:
                self.i5 = b
            elif a == 6:
                self.i6 = b
            lowerb = (self.i1, self.i2, self.i3)
            upperb = (self.i4, self.i5, self.i6)
            mask = cv.inRange(hsv, lowerb, upperb)
            cv.imshow("mask", mask)
            print(self.i1, self.i2, self.i3, self.i4, self.i5, self.i6)
    
    
        ## 调用次方法,传入文件路径
        def inRange_byFilePath(self,pathFile="img/tiger261.jpg"):
            # img = cv2.imread("data2/test_aubo2.png")
            # img_copy=img.copy()
            dstImgggg = cv.imread(pathFile)
            img_copysize = cv.resize(dstImgggg, (int(dstImgggg.shape[1] / 2), int(dstImgggg.shape[0] / 2)))
            cv.imshow("img_copysize", img_copysize)
            hsv = cv.cvtColor(img_copysize, cv.COLOR_BGR2HSV)
            cv.imshow("hsv", hsv)
            cv.namedWindow("mask")
            cv.createTrackbar("h1", "mask", 0, 255, lambda a: self.onChange(1, a, hsv))
            cv.createTrackbar("s1", "mask", 0, 255, lambda a:self.onChange(2, a, hsv))
            cv.createTrackbar("v1", "mask", 0, 255, lambda a:self.onChange(3, a, hsv))
            cv.createTrackbar("h2", "mask", 0, 255, lambda a:self.onChange(4, a, hsv))
            cv.createTrackbar("s2", "mask", 0, 255, lambda a:self.onChange(5, a, hsv))
            cv.createTrackbar("v2", "mask", 0, 255, lambda a:self.onChange(6, a, hsv), )
            cv.waitKey(0)
            cv.destroyAllWindows()
    
            ## 调用次方法,传入图片
        def inRange_byImage(self, dstImgggg):
                img_copysize = cv.resize(dstImgggg, (int(dstImgggg.shape[1] / 2), int(dstImgggg.shape[0] / 2)))
                cv.imshow("img_copysize", img_copysize)
                hsv = cv.cvtColor(img_copysize, cv.COLOR_BGR2HSV)
                cv.imshow("hsv", hsv)
                cv.namedWindow("mask")
                cv.createTrackbar("h1", "mask", 0, 255, lambda a: self.onChange(1, a, hsv))
                cv.createTrackbar("s1", "mask", 0, 255, lambda a: self.onChange(2, a, hsv))
                cv.createTrackbar("v1", "mask", 0, 255, lambda a: self.onChange(3, a, hsv))
                cv.createTrackbar("h2", "mask", 0, 255, lambda a: self.onChange(4, a, hsv))
                cv.createTrackbar("s2", "mask", 0, 255, lambda a: self.onChange(5, a, hsv))
                cv.createTrackbar("v2", "mask", 0, 255, lambda a: self.onChange(6, a, hsv), )
                cv.waitKey(0)
                cv.destroyAllWindows()
    
    
    if __name__ == '__main__':
        ##使用:
        h=Hsv_change()
        h.inRange_byFilePath("img/tiger261.jpg")

py\_os\_popen获取绝对路径

#绝对路径
    print("{}/img/output.svg".format(os.popen("pwd").readline().rstrip('\r\n')))

py\_三目运算

print(x if (x > y) else y)
    print((x if (x>y) else y) if ((x if (x>y) else y)>z) else z)

py\_str字符串查找截取替换

# 在字符串str里查找字符串hello
    str.find('hello')	
    # 将字符串里的k全部替换为8
    str.replace('k',' 8')	
    #指定分隔符为'.'
    str.split('.')
    #指定分割次数
    str.split('.',2)
    ##字符串截取,如包含点的就截取,不包含点的就不截取;
     h_index=h.find(".")
     y =  h[:h_index if (h_index>1) else len(h)]

py\_opencv原始画布与现有画布图像坐标转换

def scaleData_with_wh_svg(self,px,py,w,h):
            '''
            对数据缩放处理
            :param cal:
            :return:
            px :原图像x坐标
            py :原图像y坐标
            w  :原图像所在的画布宽度
            h  :原图像所在的画布宽度
    
            映射的画布尺寸 w:640 h: 480
            1.取最小的(高度最小) h: y_p高度尺寸,按原始画布的宽高比例计算出宽度尺寸x_p
            2.通过原始图像px坐标与原始画布w宽度的比例计算出映射画布的图像x坐标
            3.通过原始图像py坐标与原始画布h宽度的比例计算出映射画布的图像y坐标
    
            '''
            h_index=h.find(".")
            w_index = w.find(".")
            h=int(h[:h_index if (h_index>1) else len(h)])
            w=int(w[:w_index if (w_index>1) else len(w)])
            #1.取最小的(高度最小) h: y_p高度尺寸, 按原始画布的宽高比例计算出宽度尺寸x_p
            y_p = 480
            x_p = y_p * h / w
            #2.通过原始图像px坐标与原始画布w宽度的比例计算出映射画布的图像x坐标
            x =  px *x_p / h
            #3.通过原始图像py坐标与原始画布h宽度的比例计算出映射画布的图像y坐标
            y =  py * y_p/ w
            return x,y

py\_ros\_机器人写字\_svg图片绘制\_二三阶贝塞尔

# ------------------------- 绘制二阶贝塞尔 -------------------------#
        def QuadraticBezier(self,ps, pc, pe, t):
            '''
            获取二阶贝塞尔点
            :param ps: 开始点
            :param pc: 控制点
            :param pe: 结束点
            :param t: 0-1
            :return:
            '''
            return pow(1 - t, 2) * ps + 2 * t * (1 - t) * pc + pow(t, 2) * pe
    
        def drawQuadraticBezier(self,ele):
            # 开始点
            ps = np.array([ele.start.real, ele.start.imag])
            # 控制点
            p = np.array([ele.control.real, ele.control.imag])
            # 结束点
            pe = np.array([ele.end.real, ele.end.imag])
            point = self.QuadraticBezier(ps, p, pe, 0)
            start = int(point[0]), int(point[1])
            z = 30.25
            # 创建点
            point = {'x': start[0], 'y': start[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
            # 添加到容器中
            self.points.append(point)
            # 40个点
            for i in range(1, 41):
                result = self.QuadraticBezier(ps, p, pe, i / 40)
                end = int(result[0]), int(result[1])
                # 创建点
                point = {'x': end[0], 'y': end[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
                # 添加到容器中
                self.points.append(point)
                # 连接两个点
                # cv.line(self.dst, start, end, self.randomColor())
                # cv.imshow('dst', self.dst)
                # cv.waitKey(5)
                # 开始点变成结束点
                #start = end
    
        # ------------------------- 三阶贝塞尔 -------------------------#
        def CubicBezier(self,ps, pc1, pc2, pe, t):
            '''
            获取二阶贝塞尔点
            :param ps: 开始点
            :param pc: 控制点
            :param pe: 结束点
            :param t: 0-1
            :return:
            '''
            return pow(1 - t, 3) * ps + 3 * t * pow(1 - t, 2) * pc1 + 3 * pow(t, 2) * (1 - t) * pc2 + pow(t, 3) * pe
    
        def drawCubicBezier(self,ele):
            print('绘制贝塞尔')
            # 开始点
            ps = np.array([ele.start.real, ele.start.imag])
            # 控制点
            p1 = np.array([ele.control1.real, ele.control1.imag])
            p2 = np.array([ele.control2.real, ele.control2.imag])
            # 结束点
            pe = np.array([ele.end.real, ele.end.imag])
            result = self.CubicBezier(ps, p1, p2, pe, 0)
            print(result)
            start = int(result[0]), int(result[1])
            z = 30.25
            # 创建点
            point = {'x': start[0], 'y': start[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
            # 添加到容器中
            self.points.append(point)
            # 40个点
            for i in range(1, 41):
                result = self.CubicBezier(ps, p1, p2, pe, i / 40)
                end = int(result[0]), int(result[1])
                # 创建点
                point = {'x': end[0], 'y': end[1], 'z': z, 'type': TYPE.MOVE, 'pos': len(self.points)}
                # 添加到容器中
                self.points.append(point)
                # 连接两个点
                # cv.line(self.dst, start, end, self.randomColor())
                # cv.imshow('dst', self.dst)
                # cv.waitKey(5)
                # # 开始点变成结束点
                # start = end

py\_qt\_ QTimer定时器

from PyQt5.QtCore import QTimer,pyqtSignal,Qt
    
    
    
           # 自定义 timer， 负责页面更新
            update_timer = QTimer(self)
            update_timer.setInterval(16)
            update_timer.start()
            # timer事件回调
            update_timer.timeout.connect(self.on_update)
    
             def on_update(self):
                # qt刷新
                self.update()
     
                # 判断用户终止操作
                if rospy.is_shutdown():
                    self.close()

py\_os\_获取当前绝对路径

import os
    ## /home/sk/workspas/demo_ur/src/demour/scripts1
    print("{}".format(os.popen("pwd").readline().rstrip('\r\n')))

py\_ros\_**静态文件路径\_开发时python加载静态文件的路径从 .src开始往下找**

file="./src/demour/scripts1/h5/abc.h5"
    
    ##或尝试加入以下这句代码
    #sys.path.append("src/driver2/scripts")

py\_stm解析协议串口发来的16进制数据

temp = struct.unpack('h', bytearray([0xde ,0x0e]))[0]
    print(temp)
    print(0x0e<<8|0xde);
    
    temp = bytearray(struct.pack('h',int(1.02*1000)));
    print(temp)

解析串口发来的16进制数据用struct库

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1][]

py\_Serial库通过串口接收与发送数据\_解析数据

# encoding: utf-8
    import serial
    import Queue
    import threading
    import struct
    import time
    
    ser = None
    
    
    def do_imu(data):
        temp = struct.unpack('h', bytearray(data[4:6]))[0]
        print(data[4],data[5])
        # acc
        ax = struct.unpack('h', bytearray(data[6:8]))[0]
        ay = struct.unpack('h', bytearray(data[8:10]))[0]
        az = struct.unpack('h', bytearray(data[10:12]))[0]
    
        # rot
        gx = struct.unpack('h', bytearray(data[12:14]))[0]
        gy = struct.unpack('h', bytearray(data[14:16]))[0]
        gz = struct.unpack('h', bytearray(data[16:18]))[0]
    
        # mag
        mx = struct.unpack('h', bytearray(data[18:20]))[0]
        my = struct.unpack('h', bytearray(data[20:22]))[0]
        mz = struct.unpack('h', bytearray(data[22:24]))[0]
    
        v = struct.unpack('h', bytearray(data[24:26]))[0]
        w = struct.unpack('h', bytearray(data[26:28]))[0]
    
    
        print("temp={} a=({}, {}, {}) g=({}, {}, {}) m=({}, {}, {}) v={}  w={}".format(temp,ax, ay, az, gx, gy, gz, mx, my, mz,v,w))
    
    
    
    def do_parse(d_type, d_len, d_data):
    
        # 判断下位机发送过来的数据类型
        if d_type == 0x03:
            # 将头部信息插入进去封装成完整的消息帧
            d_data.insert(0, d_len)
            d_data.insert(0, d_type)
            d_data.insert(0, 0xfa)
            d_data.insert(0, 0xce)
            do_imu(d_data)
    
        elif d_type == 0x00:
            pass;
    
    
    def do_recv():
    
        while True:
            buff = recv_queue.get()
            if buff is None:
                break
            buff = bytearray(buff)[0]
            # 找到第一个头
            if buff == 0xce:
                # 找到第二个头
                buff = recv_queue.get()
                buff = bytearray(buff)[0]
                if buff == 0xfa:
                    # 走到这一步,说明两个头都匹配成功了,可以继续判断类型了
                    # 读取类型
                    ext_type = bytearray(recv_queue.get())[0]
                    # 读取长度
                    ext_len = bytearray(recv_queue.get())[0]
                    # 根据帧长度,读取所有数据
                    ext_data = []
                    while len(ext_data) < ext_len:
                        value = bytearray(recv_queue.get())[0]
                        ext_data.append(value)
                    # 解析数据
                    do_parse(ext_type,ext_len,ext_data);
    
    def do_transmit():
        
        while True:
            try:
                cmd = [0xce,0xfa,0x04]
                cmd.append(4);
                # 速度放大1000倍,避免通过小数传输
                linear_pack = bytearray(struct.pack('h',int(0.2*1000)));
                angular_pack = bytearray(struct.pack('h',int(0.5*1000)));
    
                cmd.append(linear_pack[0])
                cmd.append(linear_pack[1])
    
                cmd.append(angular_pack[0])
                cmd.append(angular_pack[1])
    
                ser.write(cmd);
    
                time.sleep(0.02);
            except Exception as e:
                print(e)
                ser.flushInput();
                ser.flushOutput();
    
    
    
    if __name__ == '__main__':
        ser = serial.Serial(port="COM25", baudrate=115200)
    
        if not ser.isOpen():
            ser.open()
    
        recv_queue = Queue.Queue()
        threading.Thread(target=do_recv).start()
        # 测试发送的线程
        threading.Thread(target=do_transmit).start()
        # 将读到的每一个字节先存到队列中
        while True:
            buff = ser.read(1)
            recv_queue.put(buff)

py\_ros\_Serial库通过串口接收与发送数据\_解析数据\_car   ===================2020=09-21

# encoding: utf-8
    import rospy
    import serial
    import Queue
    import threading
    import struct
    from geometry_msgs.msg import Twist
    
    
    def do_parse(ext_type,ext_len,ext_data):
        # 将ext_data 封装成完整的数据帧
        ext_data.insert(0, ext_len);
        ext_data.insert(0, ext_type);
        ext_data.insert(0, 0xfa);
        ext_data.insert(0,0xce);
        # CE FA 03 18    76 0E    5C 03 F0 FF 88 3C      E0 FF A2 00 84 00    00 A2 FF E0 00 84      F4 01     20 03
        # 根据数据帧的类型的类型来做对应的解析  0x01 线速度  0x02 电池
        if ext_type == 0x03:
            # 对数据进行拆包
            # 温度
            temperature = struct.unpack('h',bytearray(ext_data[4:6]))[0]
    
            # 加速度
            ax = struct.unpack('h',bytearray(ext_data[6:8]))[0]
            ay = struct.unpack('h',bytearray(ext_data[8:10]))[0]
            az = struct.unpack('h',bytearray(ext_data[10:12]))[0]
            # 角速度
            gx = struct.unpack('h', bytearray(ext_data[12:14]))[0]
            gy = struct.unpack('h', bytearray(ext_data[14:16]))[0]
            gz = struct.unpack('h', bytearray(ext_data[16:18]))[0]
            # 地磁
            mx = struct.unpack('h', bytearray(ext_data[18:20]))[0]
            my = struct.unpack('h', bytearray(ext_data[20:22]))[0]
            mz = struct.unpack('h', bytearray(ext_data[22:24]))[0]
            # 速度
            velocity = struct.unpack('h', bytearray(ext_data[24:26]))[0]
            angular = struct.unpack('h', bytearray(ext_data[26:28]))[0]
    
            # print(velocity,angular)
    
    
    def do_recv():
        """
            找到一帧完整数据 从recv_queue中查找
            1. 查找帧头0
            2. 查找帧头1
            3. 查找帧类型
            4. 查找帧的长度
            5. 根据帧长度,读取帧数据
            完整的帧数据
        """
        while True:
            buff = recv_queue.get();
            value = bytearray(buff)[0]
    
            if value == 0xce: # 找到帧头0
                # 若找到帧头0,则继续判断帧头1
                value = bytearray(recv_queue.get())[0]
                if value == 0xfa: # 若判断成功,则head0和head1匹配成功,意味着帧头完全匹配
                    # 开始读类型
                    ext_type = bytearray(recv_queue.get())[0]
                    # 读帧数据长度
                    ext_len = bytearray(recv_queue.get())[0]
                    # 读数据
                    ext_data = []
                    while len(ext_data) < ext_len:
                        # 不断往后读取数据
                        value = bytearray(recv_queue.get())[0]
                        ext_data.append(value);
    
                    # 开始解析数据
                    do_parse(ext_type,ext_len,ext_data);
    
    
    def cmd_vel_callback(msg):
        print("msg：",msg.linear.x,msg.angular.z);
        #     帧头0  帧头1  帧类型
        cmd = [0xce,0xfa,0x05]
        # 添加帧长度
        cmd.append(0x04);
    
        # 下发线速度和角速度
        velocity = msg.linear.x;
        angular = msg.angular.z;
    
        # 对数据进行拆解
        velocity_params = bytearray(struct.pack('h',int(velocity*1000)))
        angular_params = bytearray(struct.pack('h',int(angular*1000)))
    
        cmd.append(velocity_params[0])
        cmd.append(velocity_params[1])
    
        cmd.append(angular_params[0]);
        cmd.append(angular_params[1]);
    
        # [206, 250, 5, 4, 250, 0, 244, 1]
        # 将数据发送给串口
        ser.write(cmd)
    
    
    if __name__ == '__main__':
        # 创建ros节点
        node_name="car_driver_node";
        rospy.init_node(node_name);
    
        # 订阅/cmd_vel
        rospy.Subscriber("/cmd_vel",Twist,cmd_vel_callback);
    
        # 创建serial 串口通讯对象
        ser = serial.Serial(port="/dev/ttyUSB0",baudrate=115200)
    
        # 判断串口是否打开成功
        if not ser.isOpen():
            ser.open();
    
        # 创建消息队列,专门用于存储读到的数据
        recv_queue = Queue.Queue();
    
        # 开启线程,专门用于取队列中查找帧数据
        threading.Thread(target=do_recv).start();
    
        # 读取串口中的数据
        while not rospy.is_shutdown():
            buff = ser.read();
            # 读到数据,立马存起来
            recv_queue.put(buff);
    
        rospy.spin();
        ser.close();

==========================================================================================

\------------------------------------------------------------------------------------------------------------------------------------------------

# **c++模板** #

cpp\_pcl\_NormalEstimation积分图估算点云法线\_main  ------------------

//
    // Created by sk on 2020/8/5.
    //
    
    
    #include <iostream>
    #include <thread>
    #include <pcl/io/io.h>
    #include <pcl/io/pcd_io.h>
    #include <pcl/features/integral_image_normal.h>
    #include <pcl/visualization/cloud_viewer.h>
    
    using namespace std;
    
    /**
     * 积分图估算点云法线
    此方法进行法线估计只适用于有序点云,对于无序点云就只能采用其他方法。
     */
    int main(int argc, char *argv[]) {
    
    
        char *arg = argv[1];
        if (!argv[1])  arg = "./data/table_scene_mug_stereo_textured.pcd";
    
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    //    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::io::loadPCDFile(arg, *cloud);
    
        // 创建法线集合
        pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    
        // 创建一个积分图法线评估对象
        pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> iine;
    
        // 设置法线估算方式
        /**
            enum NormalEstimationMethod
    
            COVARIANCE_MATRIX : 模式创建9个积分图像，以根据其局部邻域的协方差矩阵为>特定点计算法线
            AVERAGE_3D_GRADIENT: 模式创建6个积分图像以计算水平和垂直3D渐变的平滑版本
    ，并使用这两个渐变之间的叉积计算法线。
            AVERAGE_DEPTH_CHANGE: 模式仅创建单个积分图像，并根据平均深度变化计算法线
    。
         */
        iine.setNormalEstimationMethod(iine.AVERAGE_3D_GRADIENT);
    
        // 设置最大深度变换因子
        iine.setMaxDepthChangeFactor(0.02f);
        iine.setNormalSmoothingSize(10.0f);
    
        iine.setInputCloud(cloud);
    
        iine.compute(*normals);
    
        pcl::visualization::PCLVisualizer viewer("PCLViewer");
        viewer.addPointCloud(cloud, "cloud");
        // 每几个点绘制一个法向量
        int level = 10;
        float scale = 0.02;
        viewer.addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, normals, level, scale, "normal_cloud");
    
        viewer.addCoordinateSystem(0.01);
    
        while (!viewer.wasStopped()) {
            viewer.spinOnce();
            std::this_thread::sleep_for(100ms);
        }
    
    
    }

cpp\_function包装函数\_回调函数

#include <functional>
    function<void()> connectCallBack;

cpp\_shared\_ptr<URDriver>智能指针

shared_ptr<URDriver> URDriver::instance =  shared_ptr<URDriver>(new URDriver);

cpp\_ros\_ur\_movej\_movel指令发送

//movej
    QString URScript::loadMovejScript(double *joints, double a, double v) {
        //'movej([90,80], a=1.4, v=1.05, t=0, r=0)'
        QString msg{"movej(["};
        for (int i = 0; i < 6; ++i) {
            msg += QString::number(joints[i]);
            if (i != 5) {
                msg += ",";
            }
        }
        msg += "], a=";
        msg += QString::number(a);
        msg += ", v=";
        msg += QString::number(v);
        //最后面  必要要有换行符
        msg+=", t=0, r=0)\n";
        return msg;
    }
    //movel
    QString URScript::loadMovelScript(double *pose, double a, double v) {
        //movel(pose, a=1.2, v=0.25, t=0, r=0)
        QString msg{"movel(p["};
        for (int i = 0; i < 6; ++i) {
            msg += QString::number(pose[i]);
            if (i != 5) {
                msg += ",";
            }
        }
        msg += "], a=";
        msg += QString::number(a);
        msg += ", v=";
        msg += QString::number(v);
        //最后面  必要要有换行符
        msg+=", t=0, r=0)\n";
        return msg;
    }
    
    
    //movej指令发送
    void URDriver::movej(double joints[6],double a, double v) {
        //发送指令给机械臂  'movej([90,80], a=1.4, v=1.05, t=0, r=0)'
        //1.拼接发送的指令字符串
        QString msg = script.loadMovejScript(joints,a,v);
        //2.通过socket发送拼接的字符串
        socket.write(msg.toUtf8());
    }
    //movel指令发送
    void URDriver::movel(double *pose, double a, double v) {
        //1.拼接发送的指令字符串
        QString msg = script.loadMovelScript(pose,a,v);
        //2.通过socket发送拼接的字符串
        socket.write(msg.toUtf8());
    }
    
    //movej指令发送
    void MainWindow::movej() {
        //获取6个关节角度
        double joint1 = joint1Edit->text().toDouble()*DEGREETORADIUS;
        double joint2 = joint2Edit->text().toDouble()*DEGREETORADIUS;
        double joint3 = joint3Edit->text().toDouble()*DEGREETORADIUS;
        double joint4 = joint4Edit->text().toDouble()*DEGREETORADIUS;
        double joint5 = joint5Edit->text().toDouble()*DEGREETORADIUS;
        double joint6 = joint6Edit->text().toDouble()*DEGREETORADIUS;
        //关节角度
        double joints[]{joint1,joint2,joint3,joint4,joint5,joint6};
        //移动机械臂
        URDriver::getInstance()->movej(joints);
    }
    
    //movel指令发送
    void MainWindow::movel() {
        //获取位置和姿态
        double x = xEdit->text().toDouble()/1000;
        double y = yEdit->text().toDouble()/1000;
        double z = zEdit->text().toDouble()/1000;
        //传过来的已经是弧度了,如过来的是角度的话需要转弧度 角度*DEGREETORADIUS
        double rx = rxEdit->text().toDouble();
        double ry = ryEdit->text().toDouble();
        double rz = rzEdit->text().toDouble();
        //位置和姿态
        double pose[]{x,y,z,rx,ry,rz};
        //调用驱动的movel方法
        URDriver::getInstance()->movel(pose);
    }

cpp\_opencv\_waitKey按ESC停止\_保存图片

int action = waitKey(30) & 0xFF;
            if (action == ACTION_ESC) {
                // 按下Esc停止采集，退出标定
                std::cout << "用户主动停止标定" << std::endl;
                return -1;
            } else if (action == 'q' || action == 'Q') {
                // 按下q或Q中断采集，开始执行标定
                std::cout << "停止图片采集，开始标定" << std::endl;
                break;
            } else  if (action == ACTION_SPACE) {
            std::cout << "空格" << std::endl;
            }

cpp\_opencv\_CommandLineParser从外部获取参数

int main(int argc, char **argv) {
    
        //获取外面键盘输入的参数
        cv::CommandLineParser parser(argc, argv, "{@arg1||}{@arg2||}{@arg3||}{@arg4|15|}"
                                                 "{scale s|1.0|}{flip f||}{help h||}");//导入opencv2
        if (parser.has("h")) {//从键盘获取 判断用户是否输入的h值
            printHelp();
            return 0;
        }
        int board_width = parser.get<int>(0);//从键盘获取用户输入的第一位数字
        int board_height = parser.get<int>(1);//从键盘获取用户输入的第2位数字
        int square_size = parser.get<int>(2);//从键盘获取用户输入的第3位数字
        int board_num = parser.get<int>(3); //从键盘获取用户输入的第3位数字；ps：最大图片采集数量
        float image_scale = parser.get<float>("scale");//从键盘获取用户输入的scale 的值
        bool flipHorizontal = parser.has("f");//从键盘获取用户输入的f或flip 的值
    
        if (board_width < 1 || board_height < 1 || square_size <= 0) {
            printHelp();
            return -1;
        }
    
        std::cout << "board_width: " << board_width << std::endl;
        std::cout << "board_height: " << board_height << std::endl;
        std::cout << "square_size: " << square_size << std::endl;
        std::cout << "board_num: " << board_num << std::endl;
        std::cout << "image_scale: " << image_scale << std::endl;
        std::cout << "flipHorizontal: " << flipHorizontal << std::endl;

cpp\_opencv\_flip\_resize图片镜像水平反转缩放

// 缩放
            if (image_scale != 1.0) {
                cv::resize(image0, image0, Size(), image_scale, image_scale);
            }
    
            // 水平反转
            if (flipHorizontal) {
                cv::flip(image0, image0, 1);
            }

cpp\_opencv\_FileStorage保存xml数据\_读取xml数据

//------------------------保存数据---------------------------------
    Mat cameraMatrix; // 相机内参
        Mat distCoeffs;   // 畸变系数
        Mat rvecs, tvecs; // 标定板旋转平移姿态
        // * 4. 执行标定 -> 相机内参+畸变系数
        double rms = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs,      rvecs, tvecs);
    
        std::cout << "cameraMatrix: \n" << cameraMatrix << std::endl;
        std::cout << "distCoeffs: \n" << distCoeffs << std::endl;
        std::cout << "rms: \n" << rms << std::endl;
    
        //    * 5. 保存这些内参
        time_t tm;
        time(&tm);
        struct tm *t2 = localtime(&tm);
        char buf[1024];
        strftime(buf, sizeof(buf), "%c", t2);
    
        // 图片宽高，标定版数量，标定版尺寸，格子大小，时间
    //    FileStorage fs("./calibration.xml", FileStorage::WRITE);
        FileStorage fs("./calibration.yml", FileStorage::WRITE);
        fs << "time" << buf;
        fs << "rms" << rms;
        fs << "board_count" << (int)filePaths.size();
        fs << "board_size" << patternSize;   // 标定版尺寸
        fs << "board_square" << square_size; // 格子大小
        fs << "image_width" << imageSize.width;
        fs << "image_height" << imageSize.height;
        fs << "cameraMatrix" << cameraMatrix;
        fs << "distCoeffs" << distCoeffs;
        fs.release();
    
    //------------------------读取数据---------------------------------
    
        // 加载内参
        FileStorage fs("./calibration.yml", FileStorage::READ);
        int image_width{0}, image_height{0};
        fs["image_width"] >> image_width;
        fs["image_height"] >> image_height;
    
        Mat cameraMatrix, distCoeffs;
        fs["cameraMatrix"] >> cameraMatrix;
        fs["distCoeffs"] >> distCoeffs;
        fs.release();
    
        std::cout << "image_width: " << image_width << std::endl;
        std::cout << "image_height: " << image_height << std::endl;
        std::cout << "cameraMatrix: \n" << cameraMatrix << std::endl;
        std::cout << "distCoeffs: \n" << distCoeffs << std::endl;

cpp\_opencv\_glob\_filePaths加载文件夹下所有文件\_image\_\*.jpg

vector<String> filePaths;
        cv::glob("./data/image_*.jpg", filePaths);
        for (String filePath: filePaths) {
            // 加载要去畸变图像
            Mat src = imread(filePath, IMREAD_COLOR);
    
            Mat dst;
            // 对图像进行去畸变并展示
            undistort(src, dst, cameraMatrix, distCoeffs);
    
            imshow("src", src);
            imshow("dst", dst);
    
            waitKey(0);
    
        }

cpp\_opencv\_undistort对图像去畸变

//------------------------方法一-------------------------------
      // 加载要去畸变图像
            Mat src = imread(filePath, IMREAD_COLOR);
    
            Mat dst;
            // 对图像进行去畸变并展示 src去畸变图片,dst去完畸变的图片,cameraMatrix相机内参,distCoeffs畸变系数
            undistort(src, dst, cameraMatrix, distCoeffs);
    
    
    
    
    //------------------------二-------------------------------
     // 初始化去畸变纠正变换Map
        Mat map1, map2;
        initUndistortRectifyMap(cameraMatrix, distCoeffs, Mat(),
                                cameraMatrix, image_size, CV_16SC2, map1, map2);
    
       Mat dst;
                // 对图像进行去畸变并展示
                remap(image, dst, map1, map2,
                      cv::INTER_LINEAR, cv::BORDER_CONSTANT, cv::Scalar());
    
                imshow("image", dst);

cpp\_qt\_socket\_QTcpSocket发送\_接收数据

#include <QTcpSocket>
    
    
    socket.connectToHost(ip, port);
    
     //发送指令
        socket.write(msg.toUtf8());
    
    
        //监听connected信号
        QObject::connect(&socket, &QTcpSocket::connected, [this] {
            connectCallBack();
        });
    
        //断开连接信号
        QObject::connect(&socket, &QTcpSocket::disconnected, [this] {
            disConnectCallBack();
        });
       //读取返回的数据
        QObject::connect(&socket, &QTcpSocket::readyRead, [this] {
            //读取数据
            QByteArray data = socket.readAll();
            URData urData;
            //解析数据
            parseData(data, urData);
            //决定是否执行下一条指令
            decideExcuteNextInstruction(urData);
        });

cpp\_memcpy数组赋值\_拷贝数据

#include <memory.h>
    
         char pdata[1108];
        //把data所有的数据拷贝
        memcpy(pdata, data.data(), data.size());

cpp\_bind绑定参数

void callBack(sensor_msgs::Image::ConstPtr image,const ros::Publisher *publisher){
      
        publisher->publish(image_msg);
    }
    
    
    
    
     //topic名字
        string topicName = "/usb_cam/image_raw";
        const ros::Subscriber &subscriber = node.subscribe<sensor_msgs::Image>(topicName, 10,   bind(callBack,_1,&publisher));

stm\_HAL\_GPIO\_WritePin\_RESET低电平

HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,GPIO_PIN_RESET);

stm\_HAL\_GPIO\_WritePin\_SET高电平

HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,GPIO_PIN_SET);

stm\_HAL\_Delay休眠

HAL_Delay(1000);

stm\_HAL\_ReadPin输入控制(开关点动)

//读取开关状态 实现点动
          GPIO_PinState state = HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_8);
          //控制led灯
          HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,state);

stm\_HAL\_ReadPin输入控制(开关常动)

//2.读取开关状态 实现常动
          GPIO_PinState state = HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_8);
          if(state==GPIO_PIN_RESET){
              //防止按键抖动,用户误操作
              HAL_Delay(20);
              if(!HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_8)){
                  //控制led灯 每次取反
                 HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
                 while (!HAL_GPIO_ReadPin(GPIOD, GPIO_PIN_8));
              }
          }

stm\_HAL\_UART\_Transmit接收消息

//发送消息
          char data[]="hello world!\r\n";
          // 参数1： 使用哪个usart ,参数2： 字符， 参数3：输出数量，参数4：等待时间  ;  0xffff 超时等待时间
          HAL_UART_Transmit(&huart1,data,sizeof(data)-1,3000);
          //HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1 , 0xffff);

stm\_HAL\_UART\_Receive\_Transmit接收与发送消息

// 接收的缓冲区大小
    uint8_t buffer[10]={0};
     //接收消息   参数1： 使用哪个usart ,参数2： 字符， 参数3：输出数量，参数4：等待时间  ;  0xffff 超时等待时间
        HAL_UART_Receive(&huart1,buffer,sizeof(buffer),3000);
    //HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1 , 0xffff);
        //把接收到的数据重新发送出去
        HAL_UART_Transmit(&huart1,buffer,sizeof(buffer)-1,3000);

stm\_HAL\_UART\_Receive\_IT接收与发送消息\_中断的方式进行

char buffer[20]={0};
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        //注意中断中不能写 HAL_Delay(5000); 睡眠
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART1){
    
            // 如果想让中断连续可用,需要重新开启中断
            HAL_UART_Receive_IT(&huart1,buffer,1);
            HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,GPIO_PIN_RESET);
            char data[]="callback";
            uint16_t len = sizeof(data)-1;
            // 1. 串口对象, 2.发送的数据, 3.数据的长度, 4. 超时等待时间
            HAL_UART_Transmit(&huart1,data,len,3000);
    
        }else if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            }
           // 如果想让中断连续可用,需要重新开启中断
           // HAL_UART_Receive_IT(&huart2,buffer,1);
    
    
    
    }
    
    
      /**
        * USART1中 NVIC Setings 勾选Enabled 开启中断操作
        * 重写 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 方法
        * 使用中断的方式进行配置 usart1
        * 1. 开启中断处理
        * 2. 回调函数编写业务逻辑
        * 3. 注意: 若想中断连续有效, 要在回调中重新开启中断
        * 4. 参数3 收到1个字节就进入一次中断
        */
        HAL_UART_Receive_IT(&huart1,buffer,1);

stm\_重定向printf

//重定向printf  导入头 #include <stdio.h>
    #ifdef __GNUC__
    #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    #else
    #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    #endif
    PUTCHAR_PROTOTYPE
    {
        // 参数1： 使用哪个usart ,参数2： 字符， 参数3：输出数量，参数4：超时等待时间
        HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1 , 0xffff);
        return ch;
    }
    
    
     printf("dfdsfsdfdsfdfdsfdsfd!\r\n");
    
    //-------------------------------
    
    /*
    如果想要使用printf输出浮点数的话，还需要做如下配置：
    SET(COMMON_FLAGS
        "-mcpu=cortex-m3 ${FPU_FLAGS} -mthumb -mthumb-interwork -ffunction-sections -fdata-sections \
        -g -fno-common -fmessage-length=0 -specs=nosys.specs -specs=nano.specs -u _printf_float")
    
    
    注意:改动完成之后,请在tools-->Cmake-->Reset Cache and Reload Project
    
    如果仍然无法输出浮点数,就拷贝下面文件到Src目录中
    
    /home/kaijun/STM32Cube/Repository/STM32Cube_FW_F1_V1.8.0/Projects/STM3210C_EVAL/Examples/UART/UART_Printf/syscalls.c
    
    
    */
    
    
    
    /*
    //每次我们使用printf都需要在末尾街上\r\n换行,很麻烦.所以我们可以采用如下方案,进一步进行改写.
    #define LOG_ENABLE 1
    #if LOG_ENABLE
    #define log(format,...) printf(format"\r\n",##__VA_ARGS__)
    #else
    #define log(format,...)
    #endif
    
    */

stm\_重定向printf \_log

//重定向printf  导入头 #include <stdio.h>
    #ifdef __GNUC__
    #define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
    #else
    #define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
    #endif
    //每次我们使用printf都需要在末尾街上\r\n换行,很麻烦.所以我们可以采用如下方案,进一步进行改写.
    #define LOG_ENABLE 1
    #if LOG_ENABLE
    #define log(format,...) printf(format"\r\n",##__VA_ARGS__)
    #else
    #define log(format,...)
    #endif
    
    PUTCHAR_PROTOTYPE
    {
        // 参数1： 使用哪个usart ,参数2： 字符， 参数3：输出数量，参数4：超时等待时间
        HAL_UART_Transmit(&huart1 , (uint8_t *)&ch, 1 , 0xffff);
        return ch;
    }
    
    
     printf("dfdsfsdfdsfdfdsfdsfd!");
    
        log("  _   _     _              _                     ");
        log(" (_) | |   | |            (_)                    ");
        log("  _  | |_  | |__     ___   _   _ __ ___     __ _ ");
        log(" | | | __| | '_ \\   / _ \\ | | | '_ ` _ \\   / _` |");
        log(" | | | |_  | | | | |  __/ | | | | | | | | | (_| |");
        log(" |_|  \\__| |_| |_|  \\___| |_| |_|_|_| |_|  \\__,_|");
        log("                 _               _                  ");
        log("                | |             | |              ");
        log("  _ __    ___   | |__     ___   | |_             ");
        log(" | '__|  / _ \\  | '_ \\   / _ \\  | __|            ");
        log(" | |    | (_) | | |_) | | (_) | | |_             ");
        log(" |_|     \\___/  |_.__/   \\___/   \\__|            ");
        log("                                                 ");
        log("itheima robot is started...");
    
    
    
    /*
    如果想要使用printf输出浮点数的话，还需要做如下配置：
    SET(COMMON_FLAGS
        "-mcpu=cortex-m3 ${FPU_FLAGS} -mthumb -mthumb-interwork -ffunction-sections -fdata-sections \
        -g -fno-common -fmessage-length=0 -specs=nosys.specs -specs=nano.specs -u _printf_float")
    
    
    注意:改动完成之后,请在tools-->Cmake-->Reset Cache and Reload Project
    
    如果仍然无法输出浮点数,就拷贝下面文件到Src目录中
    
    /home/kaijun/STM32Cube/Repository/STM32Cube_FW_F1_V1.8.0/Projects/STM3210C_EVAL/Examples/UART/UART_Printf/syscalls.c
    
    
    */

stm\_电机驱动

/**
       * STM32CubeMX中 tim8设置 pwm 计数值7200次;不需要设置分频系数
       * 电机驱动:
       * 1. 启动定时器 tim8  ch1
       * 2. 控制电机的正反转 a2 a3
       * 3. 控制PWM的占空比 , 高电平和低电平的比例  比较的阈值
       *  1s = 72 000 000
       *  1s=1000ms
       *  1ms = 72 000
       *  0.1ms = 7200
       */
        //1.启动定时器 开始计数
       HAL_TIM_PWM_Start(&htim8,TIM_CHANNEL_1);
       //正转  同时输入两个端子 
       HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2,GPIO_PIN_SET);
       HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3,GPIO_PIN_RESET);
      //反转
      // HAL_GPIO_WritePin(GPIOA,GPIO_PIN_2,GPIO_PIN_RESET);
      // HAL_GPIO_WritePin(GPIOA,GPIO_PIN_3,GPIO_PIN_SET);
        //设置预值,计数器的比较值 参数3: 设置 7200是一直高电平,参数3: 设置 0是一直低电平, 参数3: 设置 3600是高电平,7200-3600 是低点平
       __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,3600);

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3][]

stm\_电机驱动坨机驱动

/*   *
         * 驱动舵机
         * 1. 打开定时器: TIM1_CH1
         * 2. 配置定时器分频系数: 72-1   计数值: 20000 - 1
         * 3. 转动到0° ---> 500*/
         HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
         __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,500);
    
      while (1)
      {
           log("12344");
          // 控制舵机转动一定的角度
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,500); // 转动到 0 度
           HAL_Delay(1000);
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,1000);  // 45 度
           HAL_Delay(1000);
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,1500); // 90
           HAL_Delay(1000);
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,2000); //135
           HAL_Delay(1000);
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,2500); // 180
           HAL_Delay(1000);
           __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,5000); // 如果超出范围, 舵机工作会不正常
           HAL_Delay(1000);
          log("12344--ddd");
    
    }

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 5][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 6][]

stm\_电机驱动编码器\_得到\_分/圈

/**
     *  读取编码器数据
     *  1. 启动定时器
     *  2. 读取定时器的数据
     */
        HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_1|TIM_CHANNEL_2);
    
        // 编码器读取的频率 10HZ : 每秒变化多少次  , 每次消耗的时间
        int ENCODER_RATE = 10;
        uint32_t prev_read_time=0;
    
      while (1)
      {
    
          /**
      * 计算电机的转速 : 在单位时间内测量电机转动的圈数
      * HAL_GetTick : 获取的时间从单片机启动到当前时刻走过的毫秒值
      */
          // 单位时间
           if(HAL_GetTick() - prev_read_time > (1000/ENCODER_RATE)){
               // 读取编码器当前的值, 单位时间内变化的次数
               short count = __HAL_TIM_GET_COUNTER(&htim2);
               // 计算电机转动的圈数           圈           /     秒
    
              //   float rpm =  count*1.0/(11*90*2*2)  / (1.0/ENCODER_RATE);// 圈/秒
              // float rpm =           count*1.0/3960*ENCODER_RATE * 60;  // 圈/分钟
               //马达减速比30,是13孔
               printf("rpmd=%d \r\n",count);
               //float rpm =           count*1.0/(13*30*2*2)*ENCODER_RATE * 60;  // 圈/分钟
               float rpm =count*1.0/(1560)*ENCODER_RATE * 60;  // 圈/分钟
               // 打印浮点数 cmakelists.txt 28行 加:   -u _printf_float  注意:改动完成之后,请在tools-->Cmake-->Reset Cache and Reload Project
               printf("rpm=%f \r\n",rpm);
               // 编码器计数值重置
               __HAL_TIM_SET_COUNTER(&htim2,0);
               //时间重新赋值
               prev_read_time = HAL_GetTick();
           }

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 7][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 8][]

stm\_HAL\_GetTick时间从单片机启动到当前时刻走过的毫秒值

/**
           * 
           * HAL_GetTick : 获取的时间从单片机启动到当前时刻走过的毫秒值
           */
    
    HAL_GetTick()

stm\_HAL\_GetTick时间\_计算某一频率下的时间段

// 编码器读取的频率 50HZ : 每秒变化多少次  , 每次消耗的时间
        int ENCODER_RATE = 10;
        uint32_t prev_read_time=0;
      while (1)
      {
          /**
           * 计算电机的转速 : 在单位时间内测量电机转动的圈数
           * HAL_GetTick : 获取的时间从单片机启动到当前时刻走过的毫秒值
           */
           // 单位时间
    
           if(HAL_GetTick() - prev_read_time > (1000/ENCODER_RATE)){
    
               prev_read_time = HAL_GetTick();
           }
    }

stm\_蓝牙驱动

// 接收的缓冲区大小
    uint8_t buffer[10]={0};
    
        /**
     * 蓝牙模块转串口
     * 使用中断的方式进行配置 usart2
     * 1. 开启中断处理
     * 2. 回调函数编写业务逻辑
     * 3. 注意: 若想中断连续有效, 要在回调中重新开启中断
     */
     //开启蓝牙中断 huart2口
        HAL_UART_Receive_IT(&huart2,buffer,1);
    
    
    
    
    
    // 当读取消息的中断发生之后,就会回调这个函数
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART1){
            // 如果想让中断连续可用,需要重新开启中断
            HAL_UART_Receive_IT(&huart1,buffer,1);
    
            char data[]="callback";
            uint16_t len = sizeof(data)-1;
            // 1. 串口对象, 2.发送的数据, 3.数据的长度, 4. 超时等待时间
            HAL_UART_Transmit(&huart1,data,len,3000);
    
        }else if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            uint8_t key = buffer[0];
    
            switch (key){
                case 0x47:
                    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_10,GPIO_PIN_SET);
                    break;
                case 0x48:
                    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_10,GPIO_PIN_RESET);
                    break;
                case 0x49:
                    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,7200);
                    break;
                case 0x4A:
                    __HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,0);
                    break;
            }
           // 如果想让中断连续可用,需要重新开启中断
            HAL_UART_Receive_IT(&huart2,buffer,1);
        }
    
    }

设置为中断的方式接收数据

char buffer[20]={0};
    //重写方法
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        //注意中断中不能写 HAL_Delay(5000); 睡眠
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART1){
    
            // 如果想让中断连续可用,需要重新开启中断
            HAL_UART_Receive_IT(&huart1,buffer,1);
            HAL_GPIO_WritePin(GPIOE,GPIO_PIN_10,GPIO_PIN_RESET);
            char data[]="callback";
            uint16_t len = sizeof(data)-1;
            // 1. 串口对象, 2.发送的数据, 3.数据的长度, 4. 超时等待时间
            HAL_UART_Transmit(&huart1,data,len,3000);
    
        }else if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            }
           // 如果想让中断连续可用,需要重新开启中断
           // HAL_UART_Receive_IT(&huart2,buffer,1);
    
    
    
    }
    
    
    
     /**
        * USART1中 NVIC Setings 勾选Enabled 开启中断操作
        * 重写 void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) 方法
        * 使用中断的方式进行配置 usart1
        * 1. 开启中断处理
        * 2. 回调函数编写业务逻辑
        * 3. 注意: 若想中断连续有效, 要在回调中重新开启中断
        * 4. 参数3 收到1个字节就进入一次中断
        */
         HAL_UART_Receive_IT(&huart1,buffer,1);

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 9][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 10][]

stm\_ps2手柄遥控

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 11][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 12][]

stm\_c与c++的混合编译

#ifdef __cplusplus
    extern "C" {
    #endif
    
    //c++代码
    void hellofromCpp();
    
    /*class robot{
    
    };*/
    
    #ifdef __cplusplus
    }
    #endif

stm\_cppmain.h头\_c与c++

// 在c中使用c++ 定义宏 用c编译
    #ifdef __cplusplus
    extern "C" {
    #endif
    /**
     * 初始化的操作  在main.c 中调用 写在main方法的 while循环,外上方调用
     */
    void HeimarobotInit();
    
    /**
     * Tick机器人心脏的跳动   在main.c 中调用 写在main方法的 while循环中调用
     */
    void RobotTick();
    
    #ifdef __cplusplus
    };
    #endif

stm\_cppmain.cpp主体\_c与c++

#include "cppmain.h"
    #include <cstdio>
    #include "stm32f1xx_hal.h"
    #include "usart.h"
    
    
    /** 
     * 初始化的操作  在main.c 中调用 写在main方法的 while循环,外上方调用
     */
    void RobotInit(){
    
    }
    
    
    /**
     * Tick机器人心脏的跳动  在main.c 中调用 写在main方法的 while循环中调用
     */
    void RobotTick(){
    }

stm\_cppmain.cpp主体\_car代码实现

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "cppmain.h"
    #include <cstdio>
    #include "stm32f1xx_hal.h"
    #include "PSTwo.h"
    #include "config.h"
    #include "oled.h"
    #include "mpu9250.h"
    #include "Car.h"
    #include "usart.h"
    #include "common_uart.h"
    #include "protocol.h"
    
    
    
    // 创建手柄对象
    PSTwo psTwo(PS_DI_PORT,PS_DI_PIN,PS_DO_PORT,PS_DO_PIN,PS_CS_PORT,PS_CS_PIN,PS_CLK_PORT,PS_CLK_PIN,PS_TIM);
    Car car;
    uint8_t buffer[10]={0};
    
    // 将下位机的所有数据发送出去
    void publish_data();
    // 数据每隔一段时间对外发送
    uint32_t publish_time;
    
    /**
     * 初始化的操作
     */
    void HeimarobotInit(){
        printf("heimarobot init..\r\n");
    
        // 初始化串口通讯
        common_uart_init();
    
        //psTwo.init();
        // 初始化MPU_9250
        MPU9250_Init();
    
        // 初始化OLED
        OLED_Init();
    
        car.init();
    
        HAL_UART_Receive_IT(&huart2,buffer,1);
    }
    
    
    
    
    /**
     * Tick机器人心脏的跳动
     */
    void HeimarobotTick(){
    //    printf("heimarobot tick..\r\n");
     /*   printf("protocol=%d \r\n", sizeof(TXProtocol));
        HAL_Delay(500);
    */
    
        car.tick();
    
        publish_data();
      /*  uint8_t data[] = {0x01,0x02,0x03};
        common_uart_send(data, sizeof(data));*/
    
        //psTwo.tick();
    
        uint8_t data[]="helloworld";
        //            X, Y 坐标
        OLED_ShowString(0,0,data);
        OLED_ShowString(0,12,data);
        OLED_ShowString(0,24,data);
        // 刷新屏幕
        OLED_Refresh_Gram();
    }
    
    
    
    
    
    void publish_data(){
        // 按照一定的频率对外发送数据
        if(HAL_GetTick() - publish_time < 1000/IMU_PUSH_RATE){
            return ;
        }
        publish_time = HAL_GetTick();
    
        // 温度
        short temp = MPU_Get_Temperature();
        // 三轴加速度
        short ax,ay,az = 0;
        MPU_Get_Accelerometer(&ax,&ay,&az);
        // 三轴角速度
        short gx,gy,gz = 0;
        MPU_Get_Gyroscope(&gx,&gy,&gz);
        // 三轴地磁数据
        short mx,my,mz = 0;
        MPU_Get_Magnetometer(&mx,&my,&mz);
    
        // 将小车的线速度 ,角速度  放大1000倍的目的是为了减少数据量 , 方便处理
        short xVel = (short)(car.getXVel()*1000);
        short angularVel = (short)(car.getAngularVel()*1000);
    
        uint8_t protocolSize = sizeof(TXProtocol);
    
    
        struct TXProtocol* protocol = new TXProtocol;
        protocol->head0 = FLAG_HEAD0;
        protocol->head1 = FLAG_HEAD1;
        protocol->type = 0x03;
        protocol->len = protocolSize - 4;
        // 温度
        protocol->temperature = temp;
        // 加速度
        protocol->ax = ax;
        protocol->ay = ay;
        protocol->az = az;
        // 陀螺仪的角速度
        protocol->gx = gx;
        protocol->gy = gy;
        protocol->gz = gz;
        // 地磁
        protocol->mx = mx;
        protocol->my = my;
        protocol->mz = mz;
    
        // 线速度和角速度/**
     * 接收外部发送过来的数据
     * @param receive_buf
     * @param receive_len
     */
    void common_uart_idle_callback(uint8_t receive_buf[],uint16_t receive_len){
    
        //                类 长  低   高  低  高
        // [0xce, 0xfa, 0x05, 4, 250, 0, 244, 1]
        uint8_t i = 0;
        while(i < receive_len){
            // 先找到帧头0
            if(receive_buf[i] == FLAG_HEAD0){
                // 再找帧头1
                if(receive_buf[i+1] == FLAG_HEAD1){
                    // 若进到这里来了,说明帧头完全匹配
                    // 获取类型
                    uint8_t type = receive_buf[i+2];
                    if(type == 0x05){  // 判断是否为上位机发送过来的速度信息
                        // 获取数据长度
                        uint8_t len = receive_buf[i+3];
                        // 判断是否有足够多的数据, 为了防止获取数据越界
                        if(i + 2 + len < receive_len ){
                            // 还原线速度
                            uint8_t vel_low = receive_buf[i+4];
                            uint8_t vel_high = receive_buf[i+5];
                            short vel = vel_high<<8|vel_low;
                            float velocity = (float)vel/1000;
                            // 还原角速度
                            uint8_t angular_low = receive_buf[i+6];
                            uint8_t angular_high = receive_buf[i+7];
                            short ang = angular_high<<8|angular_low;
                            float angular = (float)ang/1000;
                            // led灯 为了表示数据在传输
                            HAL_GPIO_TogglePin(LED_PORT,LED_PIN);
                            // 为了表示数据在传输
    //                        HAL_GPIO_TogglePin(BUZZER_PORT,BUZZER_PIN);
                            car.updateVel(velocity,0,angular);
                        }
                    }
                }
            }
    
            i++;
        }
    
    }
    
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            uint8_t key = buffer[0];
    
            switch (key){
                case 0x47:
                    car.updateVel(0.2,0,0);
                    break;
                case 0x48:
                    car.updateVel(0,0,0.8);
                    break;
                case 0x49:
                    car.updateVel(0,0,0);
                    break;
                case 0x4A:
                    car.updateVel(0,0,-0.8);
                    break;
                case 0x4B:
                    car.updateVel(-0.2,0,0);
                    break;
    
                // 麦克娜姆轮
                case 0x41:
                    car.updateVel(0.2,0.2,0); // 左前方
                    break;
                case 0x42:
                    car.updateVel(0,0.2,0); // 水平向左
                    break;
                case 0x43:
                    car.updateVel(0.2,-0.2,0); // 右前方
                    break;
                case 0x44:
                    car.updateVel(-0.2,0.2,0); // 左后方
                    break;
                case 0x45:
                    car.updateVel(0,-0.2,0); // 水平向右
                    break;
                case 0x46:
                    car.updateVel(-0.2,-0.2,0); // 右后方
                    break;
                default:
                    car.updateVel(0,0,0);
                    break;
            }
            HAL_UART_Receive_IT(&huart2,buffer,1);
        }
    }

stm\_config.h宏定义

#ifndef ZXCAR_ZET6_CONFIG_H
    #define ZXCAR_ZET6_CONFIG_H
    
    #include "stm32f1xx_hal.h"
    #include "tim.h"
    //#include "i2c.h"
    #include "gpio.h"
    
    
    #ifdef __cplusplus
    extern "C" {
    #endif
    #define PI  3.1415926
    
    #define KP 1.0f
    #define KI 1.0f
    #define KD 1.0f
    
    #define CAR_MODE  2 // 默认是万向轮:1 阿克曼:2
     Serial Communication 
    #define COMM_HUART huart1
    #define COMM_HUART_REF &huart1
    #define COMM_HUART_RX huart1.hdmarx
    #define COMM_KEY    USART1
    
     protocol out 
    #define OUT_HEAD_0 0xFE
    #define OUT_HEAD_1 0xCE
    
    #define OUT_TYPE_LOG 0xF1
    #define OUT_TYPE_IMU 0x11
    #define OUT_TYPE_UNIVERSAL_VEL 0x12
    #define OUT_TYPE_BATTERY 0x13
    
     protocol in 
    #define IN_HEAD_0 0xAB
    #define IN_HEAD_1 0xBC
    
    #define IN_TYPE_LED 0x01
    #define IN_TYPE_BUZZER 0x02
    #define IN_TYPE_MOTOR 0x21
    #define IN_TYPE_UNIVERSAL_VEL 0x22
    #define IN_TYPE_SERVO 0x31
    
    #define IN_LED_CMD_CLOSE 0x00
    #define IN_LED_CMD_OPEN 0x01
    #define IN_LED_CMD_STATE 0x02
    
    #define IN_BUZZER_CMD_CLOSE 0x00
    #define IN_BUZZER_CMD_OPEN 0x01
    #define IN_BUZZER_CMD_STATE 0x02
    
     Rate //
    #define MOVE_CTRL_RATE 100
    #define IMU_PUSH_RATE 50
    #define VEL_PUSH_RATE 50
    
    /// LED //
    #define LED_PORT    GPIOE
    #define LED_PIN     GPIO_PIN_10
    
    /// Buzzer ///
    #define BUZZER_PORT     GPIOB
    #define BUZZER_PIN      GPIO_PIN_10
    
    // SWITCH 
    #define SWITCH_PORT     GPIOD
    #define SWITCH_PIN      GPIO_PIN_8
    
    / Motor back left //
    #define MOTOR1_A_GPIO_PORT     GPIOA
    #define MOTOR1_A_PIN           GPIO_PIN_3
    
    #define MOTOR1_B_GPIO_PORT     GPIOA
    #define MOTOR1_B_PIN           GPIO_PIN_2
    
    #define MOTOR1_TIM           &htim8
    #define MOTOR1_CHANNEL       TIM_CHANNEL_1
    
    #define MOTOR1_DIRECTION    1
    
    / Motor back right //
    #define MOTOR2_A_GPIO_PORT     GPIOA
    #define MOTOR2_A_PIN           GPIO_PIN_4
    
    #define MOTOR2_B_GPIO_PORT     GPIOA
    #define MOTOR2_B_PIN           GPIO_PIN_5
    
    #define MOTOR2_TIM           &htim8
    #define MOTOR2_CHANNEL       TIM_CHANNEL_2
    
    #define MOTOR2_DIRECTION    1
    
    
    / Motor front right //
    #define MOTOR3_A_GPIO_PORT     GPIOC
    #define MOTOR3_A_PIN           GPIO_PIN_5
    
    #define MOTOR3_B_GPIO_PORT     GPIOE
    #define MOTOR3_B_PIN           GPIO_PIN_12
    
    #define MOTOR3_TIM           &htim8
    #define MOTOR3_CHANNEL       TIM_CHANNEL_3
    
    #define MOTOR3_DIRECTION    1
    
    
    / Motor front left //
    #define MOTOR4_A_GPIO_PORT     GPIOB
    #define MOTOR4_A_PIN           GPIO_PIN_11
    
    #define MOTOR4_B_GPIO_PORT     GPIOB
    #define MOTOR4_B_PIN           GPIO_PIN_15
    
    #define MOTOR4_TIM           &htim8
    #define MOTOR4_CHANNEL       TIM_CHANNEL_4
    
    #define MOTOR4_DIRECTION    1
    
    / Encoder back left /
    #define ENCODER1_TIM            &htim2
    #define ENCODER1_CHANNEL        TIM_CHANNEL_ALL
    #define ENCODER1_DIRECTION      -1
    
    / Encoder back right /
    #define ENCODER2_TIM            &htim3
    #define ENCODER2_CHANNEL        TIM_CHANNEL_ALL
    #define ENCODER2_DIRECTION      1
    
    
    / Encoder front right /
    #define ENCODER3_TIM            &htim4
    #define ENCODER3_CHANNEL        TIM_CHANNEL_ALL
    #define ENCODER3_DIRECTION      -1
    
    
    / Encoder front left /
    #define ENCODER4_TIM            &htim5
    #define ENCODER4_CHANNEL        TIM_CHANNEL_ALL
    #define ENCODER4_DIRECTION      1
    
     Wheel config ///
    // 轮子转一圈的信号数
    #define WHEEL_TPR           (11 * 90 * 4)
    // 轮子的直径
    #define WHEEL_DIAMETER      0.064f
    // 左右轮子的距离
    #define WHEEL_DISTANCE      0.155f
    // 前后轮子的轴距
    #define WHEEL_AXIS          0.1445f
    
     Servo 1  pe13
    #define SERVO_K              622.8f
    #define SERVO_INIT           1500
    #define SERVO1_TIM           &htim1
    #define SERVO1_CHANNEL       TIM_CHANNEL_3
    
    /// 万向轮PID ///
    #define UNIVERSAL_KP    700.0
    #define UNIVERSAL_KI    12.0
    #define UNIVERSAL_KD    300.0
    #define MAX_PWM         7200
    #define MIN_PWM         -7200
    #define MOVE_DELAY_STOP 1000
    
    // 电池 ///
    #define MIN_VOLTAGE         10.7
    #define BATTERY_MEASURE_RATE  1
    #define BATTERY_CHECK_RATE  0.1
    
    / PS2 
    #define PS_DI_PORT      GPIOE
    #define PS_DI_PIN       GPIO_PIN_15
    #define PS_DO_PORT      GPIOD
    #define PS_DO_PIN       GPIO_PIN_3
    #define PS_CS_PORT      GPIOB
    #define PS_CS_PIN       GPIO_PIN_13
    #define PS_CLK_PORT     GPIOB
    #define PS_CLK_PIN      GPIO_PIN_12
    #define PS_TIM          &htim6
    #define PS_CHECK_RATE   15
    
    / User Key ///
    #define USER_KEY_PORT   GPIOD
    #define USER_KEY_PIN    GPIO_PIN_8
    #define USER_KEY_CHECK_RATE   20
    #define MODE_LOOP_RATE  4
    
    #ifdef __cplusplus
    }
    #endif
    
    #endif //ZXCAR_ZET6_CONFIG_H

stm\_\_ps2手柄\_PSTwo.h头\_3

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef ZXCAR_ZET6_PSTWO_H
    #define ZXCAR_ZET6_PSTWO_H
    
    #include "stm32f1xx_hal.h"
    #include "tim.h"
    #include <stdio.h>
    
    
    #ifdef __cplusplus
    
    class PSTwo {
    #define PSB_SELECT      1
    #define PSB_L3          2
    #define PSB_R3          3
    #define PSB_START       4
    #define PSB_PAD_UP      5
    #define PSB_PAD_RIGHT   6
    #define PSB_PAD_DOWN    7
    #define PSB_PAD_LEFT    8
    #define PSB_L2          9
    #define PSB_R2          10
    #define PSB_L1          11
    #define PSB_R1          12
    #define PSB_GREEN       13
    #define PSB_RED         14
    #define PSB_BLUE        15
    #define PSB_PINK        16
    
    #define PSB_TRIANGLE    13
    #define PSB_CIRCLE      14
    #define PSB_CROSS       15
    #define PSB_SQUARE      16
    
    #define PSS_RX 5                //右摇杆X轴数据
    #define PSS_RY 6                //左摇杆Y轴数据
    #define PSS_LX 7                //右摇杆X轴数据
    #define PSS_LY 8                //左摇杆Y轴数据
    
    public:
        PSTwo(){};
        PSTwo(GPIO_TypeDef *di_port, uint16_t di_pin,
              GPIO_TypeDef *do_port, uint16_t do_pin,
              GPIO_TypeDef *cs_port, uint16_t cs_pin,
              GPIO_TypeDef *clk_port, uint16_t clk_pin,
              TIM_HandleTypeDef *tim);
    
        ~PSTwo();
    
    private:
        GPIO_TypeDef *di_port;
        uint16_t di_pin;
    
        GPIO_TypeDef *do_port;
        uint16_t do_pin;
    
        GPIO_TypeDef *cs_port;
        uint16_t cs_pin;
    
        GPIO_TypeDef *clk_port;
        uint16_t clk_pin;
    
        TIM_HandleTypeDef *tim;
    
        uint8_t data[9] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    
        uint16_t mask[16] = {
                PSB_SELECT,
                PSB_L3,
                PSB_R3,
                PSB_START,
                PSB_PAD_UP,
                PSB_PAD_RIGHT,
                PSB_PAD_DOWN,
                PSB_PAD_LEFT,
                PSB_L2,
                PSB_R2,
                PSB_L1,
                PSB_R1,
                PSB_GREEN,
                PSB_RED,
                PSB_BLUE,
                PSB_PINK
        };    //按键值与按键明
    
        // 左侧摇杆的值
        uint8_t joystickLeftX;
        uint8_t joystickLeftY;
        // 右侧摇杆的值
        uint8_t joystickRightX;
        uint8_t joystickRightY;
        // 被操控的按钮的值
        uint8_t button;
    
        bool isStart = false;
    
    
    
        uint32_t update_time;
    
    
    private:
        bool isDIState();
    
        void setDO(bool high);
    
        void setCLK(bool high);
    
        void setCS(bool high);
    
        void execCmd(uint8_t cmd);
    
        void shortPoll();
    
        void enterConfig();
    
        void exitConfig();
    
        void turnOnVibrationMode();
    
        void turnOnAnalogMode();
    
        void delay(uint16_t t);
    
        void clearData();
    
        void readData();
    
        uint8_t readButtonData();
    
        uint8_t readAnalogData(uint8_t button);
    
    public:
        void init();
    
        void tick();
    
        bool isRedLightMode();
    
        void setEnable(bool flag);
        /**
         * 震动
         * @param left： 左侧电机震动
         * @param right：右侧电机震动
         */
        void vibration(uint8_t left, uint8_t right);
    };
    
    #endif
    
    
    #endif //ZXCAR_ZET6_PSTWO_H

stm\_\_ps2手柄\_PSTwo.cpp主体\_3

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "PSTwo.h"
    #include <math.h>
    #include <Legacy/stm32_hal_legacy.h>
    #include "config.h"
    
    PSTwo::PSTwo(GPIO_TypeDef *di_port, uint16_t di_pin,
                 GPIO_TypeDef *do_port, uint16_t do_pin,
                 GPIO_TypeDef *cs_port, uint16_t cs_pin,
                 GPIO_TypeDef *clk_port, uint16_t clk_pin,
                 TIM_HandleTypeDef *tim) :
            di_port(di_port), di_pin(di_pin),
            do_port(do_port), do_pin(do_pin),
            cs_port(cs_port), cs_pin(cs_pin),
            clk_port(clk_port), clk_pin(clk_pin),
            tim(tim) {
    }
    
    PSTwo::~PSTwo() {}
    
    void PSTwo::init() {
        shortPoll();
        shortPoll();
        shortPoll();
        enterConfig();
        turnOnVibrationMode();
        turnOnAnalogMode();
        exitConfig();
    }
    
    void PSTwo::setEnable(bool flag) {
        isStart = flag;
    }
    
    void PSTwo::tick() {
        if(HAL_GetTick() - update_time < PS_CHECK_RATE ){
            return;
        }
        update_time = HAL_GetTick();
    
    
        button = readButtonData();
    
        joystickLeftX = readAnalogData(PSS_LX);
        joystickLeftY = readAnalogData(PSS_LY);
        joystickRightX = readAnalogData(PSS_RX);
        joystickRightY = readAnalogData(PSS_RY);
    
    
        if (button == PSB_START && isStart == false) {
            isStart = true;
            // 第一次启动的时候,防止小车飞速
    //        joystickLeftX = 128;
    //        joystickLeftY = 128;
    //        joystickRightX = 128;
    //        joystickRightY = 128;
        }
        if (button == PSB_L2) {
            isStart = false;
        }
    
        if (!isStart) return;
    
        printf("button:%d lx=%d ly=%d rx=%d ry=%d\r\n",button,joystickLeftX,joystickLeftY,joystickRightX,joystickRightY);
    
        // 对用的按键进行判断
        switch (button){
            case PSB_PAD_UP:
                HAL_GPIO_WritePin(BUZZER_PORT,BUZZER_PIN,GPIO_PIN_SET);
                break;
            case PSB_PAD_DOWN:
                HAL_GPIO_WritePin(BUZZER_PORT,BUZZER_PIN,GPIO_PIN_RESET);
                break;
        }
    
    }
    
    
    uint8_t PSTwo::readButtonData() {
        clearData();
        readData();
    
        uint16_t key = (data[4] << 8) | data[3];     //这是16个按键  按下为0， 未按下为1
        for (int i = 0; i < 16; ++i) {
            if ((key & (1 << (mask[i] - 1))) == 0)
                return i + 1;
        }
        return 0; //没有任何按键按下
    }
    
    uint8_t PSTwo::readAnalogData(uint8_t button) {
        return data[button];
    }
    
    
    void PSTwo::readData() {
        setCS(false);
        execCmd(0x01);  //开始命令
        execCmd(0x42);  //请求数据
    
        for (int i = 2; i < 9; ++i) {
            for (int ref = 0x01; ref < 0x100; ref <<= 1) {
                setCLK(true);
                delay(5);
                setCLK(false);
                delay(5);
                setCLK(true);
                if (isDIState()) {
                    data[i] = ref | data[i];
                }
            }
            delay(16);
        }
        setCS(true);
    }
    
    void PSTwo::clearData() {
        for (unsigned char &i : data) {
            i = 0x00;
        }
    }
    
    bool PSTwo::isRedLightMode() {
        setCS(false);
        execCmd(0x01);//开始命令
        execCmd(0x42);//请求数据
        setCS(true);
    
        return data[1] != 0x73;
    }
    
    void PSTwo::vibration(uint8_t left, uint8_t right) {
        setCS(false);
        delay(16);
        execCmd(0x01);//开始命令
        execCmd(0x42);//请求数据
        execCmd(0x00);
        execCmd(left);
        execCmd(right);
        execCmd(0x00);
        execCmd(0x00);
        execCmd(0x00);
        execCmd(0x00);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::shortPoll() {
        setCS(false);
        delay(16);
        execCmd(0x01);//开始命令
        execCmd(0x42);//请求数据
        execCmd(0x00);
        execCmd(0x00);
        execCmd(0x00);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::enterConfig() {
        setCS(false);
        delay(16);
        execCmd(0x01);
        execCmd(0x43);
        execCmd(0X00);
        execCmd(0x01);
        execCmd(0x00);
        execCmd(0X00);
        execCmd(0X00);
        execCmd(0X00);
        execCmd(0X00);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::exitConfig() {
        setCS(false);
        delay(16);
        execCmd(0x01);
        execCmd(0x43);
        execCmd(0X00);
        execCmd(0x00);
        execCmd(0x5A);
        execCmd(0x5A);
        execCmd(0x5A);
        execCmd(0x5A);
        execCmd(0x5A);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::turnOnVibrationMode() {
        setCS(false);
        delay(16);
        execCmd(0x01);
        execCmd(0x4D);
        execCmd(0X00);
        execCmd(0x00);
        execCmd(0X01);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::turnOnAnalogMode() {
        setCS(false);
        execCmd(0x01);
        execCmd(0x44);
        execCmd(0X00);
        execCmd(0x01); //analog=0x01;digital=0x00  软件设置发送模式
        execCmd(0x03); //Ox03锁存设置，即不可通过按键“MODE”设置模式。
        //0xEE不锁存软件设置，可通过按键“MODE”设置模式。
        execCmd(0X00);
        execCmd(0X00);
        execCmd(0X00);
        execCmd(0X00);
        setCS(true);
        delay(16);
    }
    
    void PSTwo::setDO(bool high) {
        HAL_GPIO_WritePin(do_port, do_pin, high ? GPIO_PIN_SET : GPIO_PIN_RESET);
    }
    
    void PSTwo::setCLK(bool high) {
        HAL_GPIO_WritePin(clk_port, clk_pin, high ? GPIO_PIN_SET : GPIO_PIN_RESET);
    }
    
    void PSTwo::setCS(bool high) {
        HAL_GPIO_WritePin(cs_port, cs_pin, high ? GPIO_PIN_SET : GPIO_PIN_RESET);
    }
    
    void PSTwo::execCmd(uint8_t cmd) {
        volatile uint16_t ref = 0x01;
        data[1] = 0;
        for (ref = 0x01; ref < 0x0100; ref <<= 1) {
            if (ref & cmd) {
                setDO(true);                   //输出一位控制位
            } else {
                setDO(false);
            }
    
            setCLK(true);                        //时钟拉高
            delay(5);
            setCLK(false);
            delay(5);
            setCLK(true);
            if (isDIState()) {
                data[1] = ref | data[1];
            }
        }
        delay(16);
    }
    
    void PSTwo::delay(uint16_t t) {
        uint16_t differ = 0xffff - t - 5;
        /*为防止因中断打断延时，造成计数错误.
         如从0xfffE开始延时1us,但由于中断打断
        （此时计数器仍在计数），本因计数至0xffff）
        便停止计数，但由于错过计数值，并重载arr值，
        导致实际延时(0xffff+1)t
        */
        HAL_TIM_Base_Start(tim);
        __HAL_TIM_SetCounter(tim, differ);
    
        while (differ < 0xffff - 5) {
            differ = __HAL_TIM_GetCounter(tim);
        }
    
        HAL_TIM_Base_Stop(tim);
    }
    
    bool PSTwo::isDIState() {
        return HAL_GPIO_ReadPin(di_port, di_pin) == GPIO_PIN_SET;
    }

stm\_\_ps2手柄创建与调用\_3

#include "PSTwo.h"
    #include "config.h"
    
    // 在cppmain.cpp 中 最上方创建 pstwo对象
    PSTwo psTwo(PS_DI_PORT,PS_DI_PIN,PS_DO_PORT,PS_DO_PIN,PS_CS_PORT,PS_CS_PIN,PS_CLK_PORT,PS_CLK_PIN,PS_TIM);
    
    //在 RobotInit()中调用以下函数
    psTwo.init();
    
    //在 RobotTick()中调用以下函数
    //手柄逻辑写在psTwo.tick() 函数中
    psTwo.tick();

stm\_oled字库\_oledfont.h\_4

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_OLEDFONT_H
    #define HEIMAROBOTV4_OLEDFONT_H
    
    #ifndef __OLEDFONT_H
    #define __OLEDFONT_H
    //oled 字库
    //常用ASCII表
    //偏移量32
    //ASCII字符集
    //偏移量32
    //大小:12*6
    const unsigned char oled_asc2_1206[95][12]={
            {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*" ",0*/
            {0x00,0x00,0x00,0x00,0x3F,0x40,0x00,0x00,0x00,0x00,0x00,0x00},/*"!",1*/
            {0x00,0x00,0x30,0x00,0x40,0x00,0x30,0x00,0x40,0x00,0x00,0x00},/*""",2*/
            {0x09,0x00,0x0B,0xC0,0x3D,0x00,0x0B,0xC0,0x3D,0x00,0x09,0x00},/*"#",3*/
            {0x18,0xC0,0x24,0x40,0x7F,0xE0,0x22,0x40,0x31,0x80,0x00,0x00},/*"$",4*/
            {0x18,0x00,0x24,0xC0,0x1B,0x00,0x0D,0x80,0x32,0x40,0x01,0x80},/*"%",5*/
            {0x03,0x80,0x1C,0x40,0x27,0x40,0x1C,0x80,0x07,0x40,0x00,0x40},/*"&",6*/
            {0x10,0x00,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"'",7*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x80,0x20,0x40,0x40,0x20},/*"(",8*/
            {0x00,0x00,0x40,0x20,0x20,0x40,0x1F,0x80,0x00,0x00,0x00,0x00},/*")",9*/
            {0x09,0x00,0x06,0x00,0x1F,0x80,0x06,0x00,0x09,0x00,0x00,0x00},/*"*",10*/
            {0x04,0x00,0x04,0x00,0x3F,0x80,0x04,0x00,0x04,0x00,0x00,0x00},/*"+",11*/
            {0x00,0x10,0x00,0x60,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*",",12*/
            {0x04,0x00,0x04,0x00,0x04,0x00,0x04,0x00,0x04,0x00,0x00,0x00},/*"-",13*/
            {0x00,0x00,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*".",14*/
            {0x00,0x20,0x01,0xC0,0x06,0x00,0x38,0x00,0x40,0x00,0x00,0x00},/*"/",15*/
            {0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"0",16*/
            {0x00,0x00,0x10,0x40,0x3F,0xC0,0x00,0x40,0x00,0x00,0x00,0x00},/*"1",17*/
            {0x18,0xC0,0x21,0x40,0x22,0x40,0x24,0x40,0x18,0x40,0x00,0x00},/*"2",18*/
            {0x10,0x80,0x20,0x40,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"3",19*/
            {0x02,0x00,0x0D,0x00,0x11,0x00,0x3F,0xC0,0x01,0x40,0x00,0x00},/*"4",20*/
            {0x3C,0x80,0x24,0x40,0x24,0x40,0x24,0x40,0x23,0x80,0x00,0x00},/*"5",21*/
            {0x1F,0x80,0x24,0x40,0x24,0x40,0x34,0x40,0x03,0x80,0x00,0x00},/*"6",22*/
            {0x30,0x00,0x20,0x00,0x27,0xC0,0x38,0x00,0x20,0x00,0x00,0x00},/*"7",23*/
            {0x1B,0x80,0x24,0x40,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"8",24*/
            {0x1C,0x00,0x22,0xC0,0x22,0x40,0x22,0x40,0x1F,0x80,0x00,0x00},/*"9",25*/
            {0x00,0x00,0x00,0x00,0x08,0x40,0x00,0x00,0x00,0x00,0x00,0x00},/*":",26*/
            {0x00,0x00,0x00,0x00,0x04,0x60,0x00,0x00,0x00,0x00,0x00,0x00},/*";",27*/
            {0x00,0x00,0x04,0x00,0x0A,0x00,0x11,0x00,0x20,0x80,0x40,0x40},/*"<",28*/
            {0x09,0x00,0x09,0x00,0x09,0x00,0x09,0x00,0x09,0x00,0x00,0x00},/*"=",29*/
            {0x00,0x00,0x40,0x40,0x20,0x80,0x11,0x00,0x0A,0x00,0x04,0x00},/*">",30*/
            {0x18,0x00,0x20,0x00,0x23,0x40,0x24,0x00,0x18,0x00,0x00,0x00},/*"?",31*/
            {0x1F,0x80,0x20,0x40,0x27,0x40,0x29,0x40,0x1F,0x40,0x00,0x00},/*"@",32*/
            {0x00,0x40,0x07,0xC0,0x39,0x00,0x0F,0x00,0x01,0xC0,0x00,0x40},/*"A",33*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x24,0x40,0x1B,0x80,0x00,0x00},/*"B",34*/
            {0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x30,0x80,0x00,0x00},/*"C",35*/
            {0x20,0x40,0x3F,0xC0,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"D",36*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x2E,0x40,0x30,0xC0,0x00,0x00},/*"E",37*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x2E,0x00,0x30,0x00,0x00,0x00},/*"F",38*/
            {0x0F,0x00,0x10,0x80,0x20,0x40,0x22,0x40,0x33,0x80,0x02,0x00},/*"G",39*/
            {0x20,0x40,0x3F,0xC0,0x04,0x00,0x04,0x00,0x3F,0xC0,0x20,0x40},/*"H",40*/
            {0x20,0x40,0x20,0x40,0x3F,0xC0,0x20,0x40,0x20,0x40,0x00,0x00},/*"I",41*/
            {0x00,0x60,0x20,0x20,0x20,0x20,0x3F,0xC0,0x20,0x00,0x20,0x00},/*"J",42*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x0B,0x00,0x30,0xC0,0x20,0x40},/*"K",43*/
            {0x20,0x40,0x3F,0xC0,0x20,0x40,0x00,0x40,0x00,0x40,0x00,0xC0},/*"L",44*/
            {0x3F,0xC0,0x3C,0x00,0x03,0xC0,0x3C,0x00,0x3F,0xC0,0x00,0x00},/*"M",45*/
            {0x20,0x40,0x3F,0xC0,0x0C,0x40,0x23,0x00,0x3F,0xC0,0x20,0x00},/*"N",46*/
            {0x1F,0x80,0x20,0x40,0x20,0x40,0x20,0x40,0x1F,0x80,0x00,0x00},/*"O",47*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x24,0x00,0x18,0x00,0x00,0x00},/*"P",48*/
            {0x1F,0x80,0x21,0x40,0x21,0x40,0x20,0xE0,0x1F,0xA0,0x00,0x00},/*"Q",49*/
            {0x20,0x40,0x3F,0xC0,0x24,0x40,0x26,0x00,0x19,0xC0,0x00,0x40},/*"R",50*/
            {0x18,0xC0,0x24,0x40,0x24,0x40,0x22,0x40,0x31,0x80,0x00,0x00},/*"S",51*/
            {0x30,0x00,0x20,0x40,0x3F,0xC0,0x20,0x40,0x30,0x00,0x00,0x00},/*"T",52*/
            {0x20,0x00,0x3F,0x80,0x00,0x40,0x00,0x40,0x3F,0x80,0x20,0x00},/*"U",53*/
            {0x20,0x00,0x3E,0x00,0x01,0xC0,0x07,0x00,0x38,0x00,0x20,0x00},/*"V",54*/
            {0x38,0x00,0x07,0xC0,0x3C,0x00,0x07,0xC0,0x38,0x00,0x00,0x00},/*"W",55*/
            {0x20,0x40,0x39,0xC0,0x06,0x00,0x39,0xC0,0x20,0x40,0x00,0x00},/*"X",56*/
            {0x20,0x00,0x38,0x40,0x07,0xC0,0x38,0x40,0x20,0x00,0x00,0x00},/*"Y",57*/
            {0x30,0x40,0x21,0xC0,0x26,0x40,0x38,0x40,0x20,0xC0,0x00,0x00},/*"Z",58*/
            {0x00,0x00,0x00,0x00,0x7F,0xE0,0x40,0x20,0x40,0x20,0x00,0x00},/*"[",59*/
            {0x00,0x00,0x70,0x00,0x0C,0x00,0x03,0x80,0x00,0x40,0x00,0x00},/*"\",60*/
            {0x00,0x00,0x40,0x20,0x40,0x20,0x7F,0xE0,0x00,0x00,0x00,0x00},/*"]",61*/
            {0x00,0x00,0x20,0x00,0x40,0x00,0x20,0x00,0x00,0x00,0x00,0x00},/*"^",62*/
            {0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10,0x00,0x10},/*"_",63*/
            {0x00,0x00,0x00,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"`",64*/
            {0x00,0x00,0x02,0x80,0x05,0x40,0x05,0x40,0x03,0xC0,0x00,0x40},/*"a",65*/
            {0x20,0x00,0x3F,0xC0,0x04,0x40,0x04,0x40,0x03,0x80,0x00,0x00},/*"b",66*/
            {0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x40,0x06,0x40,0x00,0x00},/*"c",67*/
            {0x00,0x00,0x03,0x80,0x04,0x40,0x24,0x40,0x3F,0xC0,0x00,0x40},/*"d",68*/
            {0x00,0x00,0x03,0x80,0x05,0x40,0x05,0x40,0x03,0x40,0x00,0x00},/*"e",69*/
            {0x00,0x00,0x04,0x40,0x1F,0xC0,0x24,0x40,0x24,0x40,0x20,0x00},/*"f",70*/
            {0x00,0x00,0x02,0xE0,0x05,0x50,0x05,0x50,0x06,0x50,0x04,0x20},/*"g",71*/
            {0x20,0x40,0x3F,0xC0,0x04,0x40,0x04,0x00,0x03,0xC0,0x00,0x40},/*"h",72*/
            {0x00,0x00,0x04,0x40,0x27,0xC0,0x00,0x40,0x00,0x00,0x00,0x00},/*"i",73*/
            {0x00,0x10,0x00,0x10,0x04,0x10,0x27,0xE0,0x00,0x00,0x00,0x00},/*"j",74*/
            {0x20,0x40,0x3F,0xC0,0x01,0x40,0x07,0x00,0x04,0xC0,0x04,0x40},/*"k",75*/
            {0x20,0x40,0x20,0x40,0x3F,0xC0,0x00,0x40,0x00,0x40,0x00,0x00},/*"l",76*/
            {0x07,0xC0,0x04,0x00,0x07,0xC0,0x04,0x00,0x03,0xC0,0x00,0x00},/*"m",77*/
            {0x04,0x40,0x07,0xC0,0x04,0x40,0x04,0x00,0x03,0xC0,0x00,0x40},/*"n",78*/
            {0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x40,0x03,0x80,0x00,0x00},/*"o",79*/
            {0x04,0x10,0x07,0xF0,0x04,0x50,0x04,0x40,0x03,0x80,0x00,0x00},/*"p",80*/
            {0x00,0x00,0x03,0x80,0x04,0x40,0x04,0x50,0x07,0xF0,0x00,0x10},/*"q",81*/
            {0x04,0x40,0x07,0xC0,0x02,0x40,0x04,0x00,0x04,0x00,0x00,0x00},/*"r",82*/
            {0x00,0x00,0x06,0x40,0x05,0x40,0x05,0x40,0x04,0xC0,0x00,0x00},/*"s",83*/
            {0x00,0x00,0x04,0x00,0x1F,0x80,0x04,0x40,0x00,0x40,0x00,0x00},/*"t",84*/
            {0x04,0x00,0x07,0x80,0x00,0x40,0x04,0x40,0x07,0xC0,0x00,0x40},/*"u",85*/
            {0x04,0x00,0x07,0x00,0x04,0xC0,0x01,0x80,0x06,0x00,0x04,0x00},/*"v",86*/
            {0x06,0x00,0x01,0xC0,0x07,0x00,0x01,0xC0,0x06,0x00,0x00,0x00},/*"w",87*/
            {0x04,0x40,0x06,0xC0,0x01,0x00,0x06,0xC0,0x04,0x40,0x00,0x00},/*"x",88*/
            {0x04,0x10,0x07,0x10,0x04,0xE0,0x01,0x80,0x06,0x00,0x04,0x00},/*"y",89*/
            {0x00,0x00,0x04,0x40,0x05,0xC0,0x06,0x40,0x04,0x40,0x00,0x00},/*"z",90*/
            {0x00,0x00,0x00,0x00,0x04,0x00,0x7B,0xE0,0x40,0x20,0x00,0x00},/*"{",91*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xF0,0x00,0x00,0x00,0x00},/*"|",92*/
            {0x00,0x00,0x40,0x20,0x7B,0xE0,0x04,0x00,0x00,0x00,0x00,0x00},/*"}",93*/
            {0x40,0x00,0x80,0x00,0x40,0x00,0x20,0x00,0x20,0x00,0x40,0x00},/*"~",94*/
    };
    const unsigned char oled_asc2_1608[95][16]={
            {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*" ",0*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0xCC,0x00,0x0C,0x00,0x00,0x00,0x00,0x00,0x00},/*"!",1*/
            {0x00,0x00,0x08,0x00,0x30,0x00,0x60,0x00,0x08,0x00,0x30,0x00,0x60,0x00,0x00,0x00},/*""",2*/
            {0x02,0x20,0x03,0xFC,0x1E,0x20,0x02,0x20,0x03,0xFC,0x1E,0x20,0x02,0x20,0x00,0x00},/*"#",3*/
            {0x00,0x00,0x0E,0x18,0x11,0x04,0x3F,0xFF,0x10,0x84,0x0C,0x78,0x00,0x00,0x00,0x00},/*"$",4*/
            {0x0F,0x00,0x10,0x84,0x0F,0x38,0x00,0xC0,0x07,0x78,0x18,0x84,0x00,0x78,0x00,0x00},/*"%",5*/
            {0x00,0x78,0x0F,0x84,0x10,0xC4,0x11,0x24,0x0E,0x98,0x00,0xE4,0x00,0x84,0x00,0x08},/*"&",6*/
            {0x08,0x00,0x68,0x00,0x70,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"'",7*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x07,0xE0,0x18,0x18,0x20,0x04,0x40,0x02,0x00,0x00},/*"(",8*/
            {0x00,0x00,0x40,0x02,0x20,0x04,0x18,0x18,0x07,0xE0,0x00,0x00,0x00,0x00,0x00,0x00},/*")",9*/
            {0x02,0x40,0x02,0x40,0x01,0x80,0x0F,0xF0,0x01,0x80,0x02,0x40,0x02,0x40,0x00,0x00},/*"*",10*/
            {0x00,0x80,0x00,0x80,0x00,0x80,0x0F,0xF8,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x00},/*"+",11*/
            {0x00,0x01,0x00,0x0D,0x00,0x0E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*",",12*/
            {0x00,0x00,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80,0x00,0x80},/*"-",13*/
            {0x00,0x00,0x00,0x0C,0x00,0x0C,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*".",14*/
            {0x00,0x00,0x00,0x06,0x00,0x18,0x00,0x60,0x01,0x80,0x06,0x00,0x18,0x00,0x20,0x00},/*"/",15*/
            {0x00,0x00,0x07,0xF0,0x08,0x08,0x10,0x04,0x10,0x04,0x08,0x08,0x07,0xF0,0x00,0x00},/*"0",16*/
            {0x00,0x00,0x08,0x04,0x08,0x04,0x1F,0xFC,0x00,0x04,0x00,0x04,0x00,0x00,0x00,0x00},/*"1",17*/
            {0x00,0x00,0x0E,0x0C,0x10,0x14,0x10,0x24,0x10,0x44,0x11,0x84,0x0E,0x0C,0x00,0x00},/*"2",18*/
            {0x00,0x00,0x0C,0x18,0x10,0x04,0x11,0x04,0x11,0x04,0x12,0x88,0x0C,0x70,0x00,0x00},/*"3",19*/
            {0x00,0x00,0x00,0xE0,0x03,0x20,0x04,0x24,0x08,0x24,0x1F,0xFC,0x00,0x24,0x00,0x00},/*"4",20*/
            {0x00,0x00,0x1F,0x98,0x10,0x84,0x11,0x04,0x11,0x04,0x10,0x88,0x10,0x70,0x00,0x00},/*"5",21*/
            {0x00,0x00,0x07,0xF0,0x08,0x88,0x11,0x04,0x11,0x04,0x18,0x88,0x00,0x70,0x00,0x00},/*"6",22*/
            {0x00,0x00,0x1C,0x00,0x10,0x00,0x10,0xFC,0x13,0x00,0x1C,0x00,0x10,0x00,0x00,0x00},/*"7",23*/
            {0x00,0x00,0x0E,0x38,0x11,0x44,0x10,0x84,0x10,0x84,0x11,0x44,0x0E,0x38,0x00,0x00},/*"8",24*/
            {0x00,0x00,0x07,0x00,0x08,0x8C,0x10,0x44,0x10,0x44,0x08,0x88,0x07,0xF0,0x00,0x00},/*"9",25*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x0C,0x03,0x0C,0x00,0x00,0x00,0x00,0x00,0x00},/*":",26*/
            {0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x06,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*";",27*/
            {0x00,0x00,0x00,0x80,0x01,0x40,0x02,0x20,0x04,0x10,0x08,0x08,0x10,0x04,0x00,0x00},/*"<",28*/
            {0x02,0x20,0x02,0x20,0x02,0x20,0x02,0x20,0x02,0x20,0x02,0x20,0x02,0x20,0x00,0x00},/*"=",29*/
            {0x00,0x00,0x10,0x04,0x08,0x08,0x04,0x10,0x02,0x20,0x01,0x40,0x00,0x80,0x00,0x00},/*">",30*/
            {0x00,0x00,0x0E,0x00,0x12,0x00,0x10,0x0C,0x10,0x6C,0x10,0x80,0x0F,0x00,0x00,0x00},/*"?",31*/
            {0x03,0xE0,0x0C,0x18,0x13,0xE4,0x14,0x24,0x17,0xC4,0x08,0x28,0x07,0xD0,0x00,0x00},/*"@",32*/
            {0x00,0x04,0x00,0x3C,0x03,0xC4,0x1C,0x40,0x07,0x40,0x00,0xE4,0x00,0x1C,0x00,0x04},/*"A",33*/
            {0x10,0x04,0x1F,0xFC,0x11,0x04,0x11,0x04,0x11,0x04,0x0E,0x88,0x00,0x70,0x00,0x00},/*"B",34*/
            {0x03,0xE0,0x0C,0x18,0x10,0x04,0x10,0x04,0x10,0x04,0x10,0x08,0x1C,0x10,0x00,0x00},/*"C",35*/
            {0x10,0x04,0x1F,0xFC,0x10,0x04,0x10,0x04,0x10,0x04,0x08,0x08,0x07,0xF0,0x00,0x00},/*"D",36*/
            {0x10,0x04,0x1F,0xFC,0x11,0x04,0x11,0x04,0x17,0xC4,0x10,0x04,0x08,0x18,0x00,0x00},/*"E",37*/
            {0x10,0x04,0x1F,0xFC,0x11,0x04,0x11,0x00,0x17,0xC0,0x10,0x00,0x08,0x00,0x00,0x00},/*"F",38*/
            {0x03,0xE0,0x0C,0x18,0x10,0x04,0x10,0x04,0x10,0x44,0x1C,0x78,0x00,0x40,0x00,0x00},/*"G",39*/
            {0x10,0x04,0x1F,0xFC,0x10,0x84,0x00,0x80,0x00,0x80,0x10,0x84,0x1F,0xFC,0x10,0x04},/*"H",40*/
            {0x00,0x00,0x10,0x04,0x10,0x04,0x1F,0xFC,0x10,0x04,0x10,0x04,0x00,0x00,0x00,0x00},/*"I",41*/
            {0x00,0x03,0x00,0x01,0x10,0x01,0x10,0x01,0x1F,0xFE,0x10,0x00,0x10,0x00,0x00,0x00},/*"J",42*/
            {0x10,0x04,0x1F,0xFC,0x11,0x04,0x03,0x80,0x14,0x64,0x18,0x1C,0x10,0x04,0x00,0x00},/*"K",43*/
            {0x10,0x04,0x1F,0xFC,0x10,0x04,0x00,0x04,0x00,0x04,0x00,0x04,0x00,0x0C,0x00,0x00},/*"L",44*/
            {0x10,0x04,0x1F,0xFC,0x1F,0x00,0x00,0xFC,0x1F,0x00,0x1F,0xFC,0x10,0x04,0x00,0x00},/*"M",45*/
            {0x10,0x04,0x1F,0xFC,0x0C,0x04,0x03,0x00,0x00,0xE0,0x10,0x18,0x1F,0xFC,0x10,0x00},/*"N",46*/
            {0x07,0xF0,0x08,0x08,0x10,0x04,0x10,0x04,0x10,0x04,0x08,0x08,0x07,0xF0,0x00,0x00},/*"O",47*/
            {0x10,0x04,0x1F,0xFC,0x10,0x84,0x10,0x80,0x10,0x80,0x10,0x80,0x0F,0x00,0x00,0x00},/*"P",48*/
            {0x07,0xF0,0x08,0x18,0x10,0x24,0x10,0x24,0x10,0x1C,0x08,0x0A,0x07,0xF2,0x00,0x00},/*"Q",49*/
            {0x10,0x04,0x1F,0xFC,0x11,0x04,0x11,0x00,0x11,0xC0,0x11,0x30,0x0E,0x0C,0x00,0x04},/*"R",50*/
            {0x00,0x00,0x0E,0x1C,0x11,0x04,0x10,0x84,0x10,0x84,0x10,0x44,0x1C,0x38,0x00,0x00},/*"S",51*/
            {0x18,0x00,0x10,0x00,0x10,0x04,0x1F,0xFC,0x10,0x04,0x10,0x00,0x18,0x00,0x00,0x00},/*"T",52*/
            {0x10,0x00,0x1F,0xF8,0x10,0x04,0x00,0x04,0x00,0x04,0x10,0x04,0x1F,0xF8,0x10,0x00},/*"U",53*/
            {0x10,0x00,0x1E,0x00,0x11,0xE0,0x00,0x1C,0x00,0x70,0x13,0x80,0x1C,0x00,0x10,0x00},/*"V",54*/
            {0x1F,0xC0,0x10,0x3C,0x00,0xE0,0x1F,0x00,0x00,0xE0,0x10,0x3C,0x1F,0xC0,0x00,0x00},/*"W",55*/
            {0x10,0x04,0x18,0x0C,0x16,0x34,0x01,0xC0,0x01,0xC0,0x16,0x34,0x18,0x0C,0x10,0x04},/*"X",56*/
            {0x10,0x00,0x1C,0x00,0x13,0x04,0x00,0xFC,0x13,0x04,0x1C,0x00,0x10,0x00,0x00,0x00},/*"Y",57*/
            {0x08,0x04,0x10,0x1C,0x10,0x64,0x10,0x84,0x13,0x04,0x1C,0x04,0x10,0x18,0x00,0x00},/*"Z",58*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x7F,0xFE,0x40,0x02,0x40,0x02,0x40,0x02,0x00,0x00},/*"[",59*/
            {0x00,0x00,0x30,0x00,0x0C,0x00,0x03,0x80,0x00,0x60,0x00,0x1C,0x00,0x03,0x00,0x00},/*"\",60*/
            {0x00,0x00,0x40,0x02,0x40,0x02,0x40,0x02,0x7F,0xFE,0x00,0x00,0x00,0x00,0x00,0x00},/*"]",61*/
            {0x00,0x00,0x00,0x00,0x20,0x00,0x40,0x00,0x40,0x00,0x40,0x00,0x20,0x00,0x00,0x00},/*"^",62*/
            {0x00,0x01,0x00,0x01,0x00,0x01,0x00,0x01,0x00,0x01,0x00,0x01,0x00,0x01,0x00,0x01},/*"_",63*/
            {0x00,0x00,0x40,0x00,0x40,0x00,0x20,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"`",64*/
            {0x00,0x00,0x00,0x98,0x01,0x24,0x01,0x44,0x01,0x44,0x01,0x44,0x00,0xFC,0x00,0x04},/*"a",65*/
            {0x10,0x00,0x1F,0xFC,0x00,0x88,0x01,0x04,0x01,0x04,0x00,0x88,0x00,0x70,0x00,0x00},/*"b",66*/
            {0x00,0x00,0x00,0x70,0x00,0x88,0x01,0x04,0x01,0x04,0x01,0x04,0x00,0x88,0x00,0x00},/*"c",67*/
            {0x00,0x00,0x00,0x70,0x00,0x88,0x01,0x04,0x01,0x04,0x11,0x08,0x1F,0xFC,0x00,0x04},/*"d",68*/
            {0x00,0x00,0x00,0xF8,0x01,0x44,0x01,0x44,0x01,0x44,0x01,0x44,0x00,0xC8,0x00,0x00},/*"e",69*/
            {0x00,0x00,0x01,0x04,0x01,0x04,0x0F,0xFC,0x11,0x04,0x11,0x04,0x11,0x00,0x18,0x00},/*"f",70*/
            {0x00,0x00,0x00,0xD6,0x01,0x29,0x01,0x29,0x01,0x29,0x01,0xC9,0x01,0x06,0x00,0x00},/*"g",71*/
            {0x10,0x04,0x1F,0xFC,0x00,0x84,0x01,0x00,0x01,0x00,0x01,0x04,0x00,0xFC,0x00,0x04},/*"h",72*/
            {0x00,0x00,0x01,0x04,0x19,0x04,0x19,0xFC,0x00,0x04,0x00,0x04,0x00,0x00,0x00,0x00},/*"i",73*/
            {0x00,0x00,0x00,0x03,0x00,0x01,0x01,0x01,0x19,0x01,0x19,0xFE,0x00,0x00,0x00,0x00},/*"j",74*/
            {0x10,0x04,0x1F,0xFC,0x00,0x24,0x00,0x40,0x01,0xB4,0x01,0x0C,0x01,0x04,0x00,0x00},/*"k",75*/
            {0x00,0x00,0x10,0x04,0x10,0x04,0x1F,0xFC,0x00,0x04,0x00,0x04,0x00,0x00,0x00,0x00},/*"l",76*/
            {0x01,0x04,0x01,0xFC,0x01,0x04,0x01,0x00,0x01,0xFC,0x01,0x04,0x01,0x00,0x00,0xFC},/*"m",77*/
            {0x01,0x04,0x01,0xFC,0x00,0x84,0x01,0x00,0x01,0x00,0x01,0x04,0x00,0xFC,0x00,0x04},/*"n",78*/
            {0x00,0x00,0x00,0xF8,0x01,0x04,0x01,0x04,0x01,0x04,0x01,0x04,0x00,0xF8,0x00,0x00},/*"o",79*/
            {0x01,0x01,0x01,0xFF,0x00,0x85,0x01,0x04,0x01,0x04,0x00,0x88,0x00,0x70,0x00,0x00},/*"p",80*/
            {0x00,0x00,0x00,0x70,0x00,0x88,0x01,0x04,0x01,0x04,0x01,0x05,0x01,0xFF,0x00,0x01},/*"q",81*/
            {0x01,0x04,0x01,0x04,0x01,0xFC,0x00,0x84,0x01,0x04,0x01,0x00,0x01,0x80,0x00,0x00},/*"r",82*/
            {0x00,0x00,0x00,0xCC,0x01,0x24,0x01,0x24,0x01,0x24,0x01,0x24,0x01,0x98,0x00,0x00},/*"s",83*/
            {0x00,0x00,0x01,0x00,0x01,0x00,0x07,0xF8,0x01,0x04,0x01,0x04,0x00,0x00,0x00,0x00},/*"t",84*/
            {0x01,0x00,0x01,0xF8,0x00,0x04,0x00,0x04,0x00,0x04,0x01,0x08,0x01,0xFC,0x00,0x04},/*"u",85*/
            {0x01,0x00,0x01,0x80,0x01,0x70,0x00,0x0C,0x00,0x10,0x01,0x60,0x01,0x80,0x01,0x00},/*"v",86*/
            {0x01,0xF0,0x01,0x0C,0x00,0x30,0x01,0xC0,0x00,0x30,0x01,0x0C,0x01,0xF0,0x01,0x00},/*"w",87*/
            {0x00,0x00,0x01,0x04,0x01,0x8C,0x00,0x74,0x01,0x70,0x01,0x8C,0x01,0x04,0x00,0x00},/*"x",88*/
            {0x01,0x01,0x01,0x81,0x01,0x71,0x00,0x0E,0x00,0x18,0x01,0x60,0x01,0x80,0x01,0x00},/*"y",89*/
            {0x00,0x00,0x01,0x84,0x01,0x0C,0x01,0x34,0x01,0x44,0x01,0x84,0x01,0x0C,0x00,0x00},/*"z",90*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x3E,0xFC,0x40,0x02,0x40,0x02},/*"{",91*/
            {0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00},/*"|",92*/
            {0x00,0x00,0x40,0x02,0x40,0x02,0x3E,0xFC,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00},/*"}",93*/
            {0x00,0x00,0x60,0x00,0x80,0x00,0x80,0x00,0x40,0x00,0x40,0x00,0x20,0x00,0x20,0x00},/*"~",94*/
    };
    
    
    
    unsigned char Hzk1[22][16]={
            {0x00,0x00,0x3E,0x22,0x2A,0x32,0x22,0xFE,0x22,0x32,0x2A,0x22,0x3E,0x00,0x00,0x00},
            {0x88,0x68,0x09,0x09,0x29,0xC9,0x09,0x0F,0x29,0xC9,0x09,0x09,0x29,0xC8,0x08,0x00},/*"黑",0*/
    /* (16 X 16 , 宋体 )*/
    
            {0x00,0x02,0x02,0xF2,0x82,0x82,0x82,0x82,0x82,0x82,0xFE,0x80,0x80,0x80,0x00,0x00},
            {0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x08,0x48,0x80,0x40,0x3F,0x00,0x00},/*"马",1*/
    /* (16 X 16 , 宋体 )*/
    
            {0x24,0x24,0xA4,0xFE,0x23,0x22,0x00,0x3E,0x22,0x22,0x22,0x22,0x22,0x3E,0x00,0x00},
            {0x08,0x06,0x01,0xFF,0x01,0x06,0x40,0x49,0x49,0x49,0x7F,0x49,0x49,0x49,0x41,0x00},/*"程",2*/
    /* (16 X 16 , 宋体 )*/
    
            {0x00,0x00,0xFC,0x04,0x04,0x04,0x14,0x15,0x56,0x94,0x54,0x34,0x14,0x04,0x04,0x00},
            {0x40,0x30,0x0F,0x00,0x01,0x01,0x01,0x41,0x81,0x7F,0x01,0x01,0x01,0x05,0x03,0x00},/*"序",3*/
    /* (16 X 16 , 宋体 )*/
    
            {0x00,0x00,0xC0,0x5E,0x52,0x52,0x52,0x52,0x52,0x52,0x52,0x5E,0xC0,0x00,0x00,0x00},
            {0x80,0x80,0x8F,0x40,0x40,0x20,0x10,0x0E,0x10,0x10,0x20,0x20,0x4F,0x80,0x00,0x00},/*"员",4*/
    /* (16 X 16 , 宋体 )*/
    
            {0x10,0x10,0xD0,0xFF,0x90,0x10,0x00,0xFE,0x02,0x02,0x02,0xFE,0x00,0x00,0x00,0x00},
            {0x04,0x03,0x00,0xFF,0x00,0x83,0x60,0x1F,0x00,0x00,0x00,0x3F,0x40,0x40,0x78,0x00},/*"机",5*/
    /* (16 X 16 , 宋体 )*/
    
            {0x80,0x80,0x9E,0x92,0x92,0x92,0x9E,0xE0,0x80,0x9E,0xB2,0xD2,0x92,0x9E,0x80,0x00},
            {0x08,0x08,0xF4,0x94,0x92,0x92,0xF1,0x00,0x01,0xF2,0x92,0x94,0x94,0xF8,0x08,0x00},/*"器",6*/
    /* (16 X 16 , 宋体 )*/
    
            {0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0x3F,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00},
            {0x80,0x40,0x20,0x10,0x0C,0x03,0x00,0x00,0x00,0x03,0x0C,0x10,0x20,0x40,0x80,0x00},/*"人",7*/
    /* (16 X 16 , 宋体 )*/
    
            {0x04,0x84,0xE4,0x5C,0x44,0xC4,0x80,0x82,0xFE,0x82,0x82,0x82,0xFE,0x82,0x80,0x00},
            {0x02,0x01,0x7F,0x10,0x10,0x3F,0x80,0x60,0x1F,0x00,0x00,0x00,0xFF,0x00,0x00,0x00},/*"研",8*/
    /* (16 X 16 , 宋体 )*/
    
            {0x10,0x0C,0x44,0x24,0x14,0x04,0xE5,0x06,0x04,0x04,0x14,0x24,0x44,0x14,0x0C,0x00},
            {0x00,0x80,0x41,0x21,0x11,0x0D,0x03,0x01,0x01,0x01,0x3F,0x40,0x40,0x40,0x70,0x00},/*"究",9*/
    /* (16 X 16 , 宋体 )*/
    
            {0x00,0xFE,0x22,0x5A,0x86,0x10,0x0C,0x24,0x24,0x25,0x26,0x24,0x24,0x14,0x0C,0x00},
            {0x00,0xFF,0x04,0x08,0x07,0x80,0x41,0x31,0x0F,0x01,0x01,0x3F,0x41,0x41,0x71,0x00},/*"院",10*/
    /* (16 X 16 , 宋体 )*/
    };
    #endif
    
    #endif //HEIMAROBOTV4_OLEDFONT_H

stm\_oled.h头\_4

//
    // Created by Kaijun on 2020/9/11.
    //
    #ifndef __OLED_H
    #define __OLED_H
    
    #ifdef __cplusplus
    extern "C" {
    #endif
    
    #include "stm32f1xx_hal.h"
    
    #define u32 uint32_t
    #define u8 uint8_t
    //-----------------OLED端口定义----------------
    #define OLED_RST_Clr() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7,GPIO_PIN_RESET) //PEout(7)=0   //RST
    #define OLED_RST_Set() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_7,GPIO_PIN_SET) //PEout(7)=1   //RST
    
    #define OLED_RS_Clr() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_8,GPIO_PIN_RESET) // PEout(8)=0    //DC
    #define OLED_RS_Set() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_8,GPIO_PIN_SET) //PEout(8)=1    //DC
    
    #define OLED_SCLK_Clr() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_5,GPIO_PIN_RESET) // PEout(5)=0  //SCL
    #define OLED_SCLK_Set() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_5,GPIO_PIN_SET) // PEout(5)=1   //SCL
    
    #define OLED_SDIN_Clr() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,GPIO_PIN_RESET) // PEout(6)=0   //SDA
    #define OLED_SDIN_Set() HAL_GPIO_WritePin(GPIOE,GPIO_PIN_6,GPIO_PIN_SET) // PEout(6)=1   //SDA
    
    #define OLED_CMD  0    //写命令
    #define OLED_DATA 1    //写数据
    
    //OLED控制用函数
    void OLED_WR_Byte(u8 dat, u8 cmd);
    
    void OLED_Display_On(void);
    
    void OLED_Display_Off(void);
    
    void OLED_Refresh_Gram(void);
    
    void OLED_Init(void);
    
    void OLED_Clear(void);
    
    void OLED_DrawPoint(u8 x, u8 y, u8 t);
    
    void OLED_ShowChar(u8 x, u8 y, u8 chr, u8 size, u8 mode);
    
    void OLED_ShowNumber(u8 x, u8 y, u32 num, u8 len, u8 size);
    
    void OLED_ShowString(u8 x, u8 y, const u8 *p);
    
    void OLED_ShowChinese(u8 x,u8 y,u8 num,u8 size1);
    
    
    #ifdef __cplusplus
    }
    #endif
    #endif

stm\_oled.c主体\_4

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "oled.h"
    #include "oled.h"
    #include "stdlib.h"
    #include "oledfont.h"
    
    u8 OLED_GRAM[128][8];
    void OLED_Refresh_Gram(void)
    {
        u8 i,n;
        for(i=0;i<8;i++)
        {
            OLED_WR_Byte (0xb0+i,OLED_CMD);    //设置页地址（0~7）
            OLED_WR_Byte (0x00,OLED_CMD);      //设置显示位置—列低地址
            OLED_WR_Byte (0x10,OLED_CMD);      //设置显示位置—列高地址
            for(n=0;n<128;n++)OLED_WR_Byte(OLED_GRAM[n][i],OLED_DATA);
        }
    }
    /**************************************************************************
    函数功能：向OLED写入一个字节
    入口参数?//dat:要写入的数据/命令    //cmd:数据/命令标志 0,表示命令;1,表示数据;
    返回  值：无
    **************************************************************************/
    void OLED_WR_Byte(u8 dat,u8 cmd)
    {
        u8 i;
        if(cmd)
            OLED_RS_Set();
        else
            OLED_RS_Clr();
        for(i=0;i<8;i++)
        {
            OLED_SCLK_Clr();
            if(dat&0x80)
                OLED_SDIN_Set();
            else
                OLED_SDIN_Clr();
            OLED_SCLK_Set();
            dat<<=1;
        }
        OLED_RS_Set();
    }
    /**************************************************************************
    函数功能：开启OLED显示
    入口参数：//x,y :起点坐标    //t:1 填充 0,清空
    返回  值：无
    **************************************************************************/
    void OLED_Display_On(void)
    {
        OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令
        OLED_WR_Byte(0X14,OLED_CMD);  //DCDC ON
        OLED_WR_Byte(0XAF,OLED_CMD);  //DISPLAY ON
    }
    /**************************************************************************
    函数功能：关闭OLED显示
    入口参数：//x,y :起点坐标    //t:1 填充 0,清空
    返回  值：无
    **************************************************************************/
    void OLED_Display_Off(void)
    {
        OLED_WR_Byte(0X8D,OLED_CMD);  //SET DCDC命令
        OLED_WR_Byte(0X10,OLED_CMD);  //DCDC OFF
        OLED_WR_Byte(0XAE,OLED_CMD);  //DISPLAY OFF
    }
    /**************************************************************************
    函数功能：清屏函数,清完屏,整个屏幕是黑色的!和没点亮一样!!!
    入口参数：//x,y :起点坐标    //t:1 填充 0,清空
    返回  值：无
    **************************************************************************/
    void OLED_Clear(void)
    {
        u8 i,n;
        for(i=0;i<8;i++)for(n=0;n<128;n++)OLED_GRAM[n][i]=0X00;
        OLED_Refresh_Gram();//更新显示
    }
    /**************************************************************************
    函数功能：画点
    入口参数：//x,y :起点坐标    //t:1 填充 0,清空
    返回  值：无
    **************************************************************************/
    void OLED_DrawPoint(u8 x,u8 y,u8 t)
    {
        u8 pos,bx,temp=0;
        if(x>127||y>63)return;//超出范围了.
        pos=7-y/8;
        bx=y%8;
        temp=1<<(7-bx);
        if(t)OLED_GRAM[x][pos]|=temp;
        else OLED_GRAM[x][pos]&=~temp;
    }
    /**************************************************************************
    函数功能：在指定位置显示一个字符,包括部分字符
    入口参数：//x,y :起点坐标    //len :数字的位数  //size:字体大小 // mode:0,反白显示;1,正常显示
    返回  值：无
    **************************************************************************/
    void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size,u8 mode)
    {
        u8 temp,t,t1;
        u8 y0=y;
        chr=chr-' ';//得到偏移后的值
        for(t=0;t<size;t++)
        {
            if(size==12)temp=oled_asc2_1206[chr][t];  //调用1206字体
            else temp=oled_asc2_1608[chr][t];        //调用1608字体
            for(t1=0;t1<8;t1++)
            {
                if(temp&0x80)OLED_DrawPoint(x,y,mode);
                else OLED_DrawPoint(x,y,!mode);
                temp<<=1;
                y++;
                if((y-y0)==size)
                {
                    y=y0;
                    x++;
                    break;
                }
            }
        }
    }
    /**************************************************************************
    函数功能：求m的n次方的函数
    入口参数：m：底数，n：次方数
    返回  值：无
    **************************************************************************/
    u32 oled_pow(u8 m,u8 n)
    {
        u32 result=1;
        while(n--)result*=m;
        return result;
    }
    
    /**************************************************************************
    函数功能：显示2个数字
    入口参数：//x,y :起点坐标    //len :数字的位数  //size:字体大小  //mode:模式    0,填充模式;1,叠加模式  //num:数值(0~4294967295);
    返回  值：无
    **************************************************************************/
    void OLED_ShowNumber(u8 x,u8 y,u32 num,u8 len,u8 size)
    {
        u8 t,temp;
        u8 enshow=0;
        for(t=0;t<len;t++)
        {
            temp=(num/oled_pow(10,len-t-1))%10;
            if(enshow==0&&t<(len-1))
            {
                if(temp==0)
                {
                    OLED_ShowChar(x+(size/2)*t,y,' ',size,1);
                    continue;
                }else enshow=1;
    
            }
            OLED_ShowChar(x+(size/2)*t,y,temp+'0',size,1);
        }
    }
    /**************************************************************************
    函数功能：显示字符串
    入口参数：//x,y :起点坐标     // *p:字符串起始地址
    返回  值：无
    **************************************************************************/
    void OLED_ShowString(u8 x,u8 y,const u8 *p)
    {
    #define MAX_CHAR_POSX 122
    #define MAX_CHAR_POSY 58
        while(*p!='\0')
        {
            if(x>MAX_CHAR_POSX){x=0;y+=16;}
            if(y>MAX_CHAR_POSY){y=x=0;OLED_Clear();}
            OLED_ShowChar(x,y,*p,12,1);
            x+=8;
            p++;
        }
    }
    
    
    //清除一个点
    //x:0~127
    //y:0~63
    void OLED_ClearPoint(u8 x,u8 y)
    {
        u8 i,m,n;
        i=y/8;
        m=y%8;
        n=1<<m;
        OLED_GRAM[x][i]=~OLED_GRAM[x][i];
        OLED_GRAM[x][i]|=n;
        OLED_GRAM[x][i]=~OLED_GRAM[x][i];
    }
    //显示汉字
    //x,y:起点坐标
    //num:汉字对应的序号
    void OLED_ShowChinese(u8 x,u8 y,u8 num,u8 size1)
    {
        u8 i,m,n=0,temp,chr1;
        u8 x0=x,y0=y;
        u8 size3=size1/8;
        while(size3--)
        {
            chr1=num*size1/8+n;
            n++;
            for(i=0;i<size1;i++)
            {
                if(size1==16)
                {temp=Hzk1[chr1][i];}//调用16*16字体
                    /* else if(size1==24)
                     {temp=Hzk2[chr1][i];}//调用24*24字体
                     else if(size1==32)
                     {temp=Hzk3[chr1][i];}//调用32*32字体
                     else if(size1==64)
                     {temp=Hzk4[chr1][i];}//调用64*64字体*/
                else return;
    
                for(m=0;m<8;m++)
                {
                    if(temp&0x01)OLED_DrawPoint(x,y,1);
                    else OLED_ClearPoint(x,y);
                    temp>>=1;
                    y++;
                }
                x++;
                if((x-x0)==size1)
                {x=x0;y0=y0+8;}
                y=y0;
            }
        }
    }
    
    //num 显示汉字的个数
    //space 每一遍显示的间隔
    void OLED_ScrollDisplay(u8 num,u8 space)
    {
        u8 i,n,t=0,m=0,r;
        while(1)
        {
            if(m==0)
            {
                OLED_ShowChinese(128,24,t,16); //写入一个汉字保存在OLED_GRAM[][]数组中
                t++;
            }
            if(t==num)
            {
                for(r=0;r<16*space;r++)      //显示间隔
                {
                    for(i=0;i<144;i++)
                    {
                        for(n=0;n<8;n++)
                        {
                            OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
                        }
                    }
                    OLED_Refresh_Gram();
                }
                t=0;
            }
            m++;
            if(m==16){
                m=0;
            }
            for(i=0;i<144;i++)   //实现左移
            {
                for(n=0;n<8;n++)
                {
                    OLED_GRAM[i-1][n]=OLED_GRAM[i][n];
                }
            }
            OLED_Refresh_Gram();
        }
    }
    /**************************************************************************
    函数功能：初始化OLED
    入口参数无
    返回  值：无
    **************************************************************************/
    void OLED_Init(void)
    {
    //  GPIO_InitTypeDef GPIO_InitStructure;
    //  RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOE, ENABLE); //使能PB端口时钟
    //  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7|GPIO_Pin_8;//端口配置
    //  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;      //推挽输出
    //  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;      //2M
    //  GPIO_Init(GPIOE, &GPIO_InitStructure);                        //根据设定参数初始化GPIO
    //
    //
    //  PWR_BackupAccessCmd(ENABLE);//允许修改RTC 和后备寄存器
    //  RCC_LSEConfig(RCC_LSE_OFF);//关闭外部低速外部时钟信号功能 后，PC13 PC14 PC15 才可以当普通IO用。
    //  BKP_TamperPinCmd(DISABLE);//关闭入侵检测功能，也就是 PC13，也可以当普通IO 使用
    //  PWR_BackupAccessCmd(DISABLE);//禁止修改后备寄存器
    
        OLED_RST_Clr();
        HAL_Delay(100);
        OLED_RST_Set();
    
        OLED_WR_Byte(0xAE,OLED_CMD); //关闭显示
        OLED_WR_Byte(0xD5,OLED_CMD); //设置时钟分频因子,震荡频率
        OLED_WR_Byte(80,OLED_CMD);   //[3:0],分频因子;[7:4],震荡频率
        OLED_WR_Byte(0xA8,OLED_CMD); //设置驱动路数
        OLED_WR_Byte(0X3F,OLED_CMD); //默认0X3F(1/64)
        OLED_WR_Byte(0xD3,OLED_CMD); //设置显示偏移
        OLED_WR_Byte(0X00,OLED_CMD); //默认为0
    
        OLED_WR_Byte(0x40,OLED_CMD); //设置显示开始行 [5:0],行数.
    
        OLED_WR_Byte(0x8D,OLED_CMD); //电荷泵设置
        OLED_WR_Byte(0x14,OLED_CMD); //bit2，开启/关闭
        OLED_WR_Byte(0x20,OLED_CMD); //设置内存地址模式
        OLED_WR_Byte(0x02,OLED_CMD); //[1:0],00，列地址模式;01，行地址模式;10,页地址模式;默认10;
        OLED_WR_Byte(0xA1,OLED_CMD); //段重定义设置,bit0:0,0->0;1,0->127;
        OLED_WR_Byte(0xC0,OLED_CMD); //设置COM扫描方向;bit3:0,普通模式;1,重定义模式 COM[N-1]->COM0;N:驱动路数
        OLED_WR_Byte(0xDA,OLED_CMD); //设置COM硬件引脚配置
        OLED_WR_Byte(0x12,OLED_CMD); //[5:4]配置
    
        OLED_WR_Byte(0x81,OLED_CMD); //对比度设置
        OLED_WR_Byte(0xEF,OLED_CMD); //1~255;默认0X7F (亮度设置,越大越亮)
        OLED_WR_Byte(0xD9,OLED_CMD); //设置预充电周期
        OLED_WR_Byte(0xf1,OLED_CMD); //[3:0],PHASE 1;[7:4],PHASE 2;
        OLED_WR_Byte(0xDB,OLED_CMD); //设置VCOMH 电压倍率
        OLED_WR_Byte(0x30,OLED_CMD); //[6:4] 000,0.65*vcc;001,0.77*vcc;011,0.83*vcc;
    
        OLED_WR_Byte(0xA4,OLED_CMD); //全局显示开启;bit0:1,开启;0,关闭;(白屏/黑屏)
        OLED_WR_Byte(0xA6,OLED_CMD); //设置显示方式;bit0:1,反相显示;0,正常显示
        OLED_WR_Byte(0xAF,OLED_CMD); //开启显示
        OLED_Clear();
    }

stm\_oled调用\_4

#include "oled.h"  
     
     // 初始化OLED  在main.cpp init中调用
        OLED_Init();
    
      
    
      // 在main.cpp tick中调用显示数据
      uint8_t data[]="helloworld";
        //            X, Y 坐标
        OLED_ShowString(0,0,data);
        OLED_ShowString(0,12,data);
        OLED_ShowString(0,24,data);
    
        // 刷新屏幕
        OLED_Refresh_Gram();

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 13][]

stm\_陀螺仪调用\_I2C通讯\_3

#include "mpu9250.h"
     
     // 初始化  在main.cpp init中调用
    
       // 初始化MPU_9250 陀螺仪
        MPU9250_Init();
    
      // 在main.cpp tick中调用显示数据
        //陀螺仪
        getIMU();
    
    
    
    //陀螺仪
    void getIMU(){
        //获取温度
        short temp = MPU_Get_Temperature();
    
        // 三轴加速度
        short ax,ay,az;
        MPU_Get_Accelerometer(&ax,&ay,&az);
        // 三轴角速度
        short gx,gy,gz;
        MPU_Get_Gyroscope(&gx,&gy,&gz);
        // 三轴地磁数据
        short mx,my,mz;
        MPU_Get_Magnetometer(&mx,&my,&mz);
    
        printf("temperature=%d ax=%d,ay=%d,az=%d,gx=%d gy=%d gz=%d mx=%d,my=%d mz=%d \r\n",temp,ax,ay,az,gx,gy,gz,mx,my,mz);
        HAL_Delay(100);
    }

stm\_陀螺仪mpu9250.h头\_I2C通讯

//
    // Created by Kaijun on 2020/9/11.
    //
    #ifndef _MPU9250_H
    #define _MPU9250_H
    #include <stdint.h>
    
    //
    //如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
    //如果接V3.3,则IIC地址为0X69(不包含最低位).
    #define MPU9250_ADDR            0X68    //MPU9250的器件IIC地址
    #define MPU6500_ID              0X71    //MPU6500的器件ID
    
    //MPU9250内部封装了一个AK8963磁力计,地址和ID如下:
    #define AK8963_ADDR             0X0C    //AK8963的I2C地址
    #define AK8963_ID               0X48    //AK8963的器件ID
    
    
    //AK8963的内部寄存器
    #define MAG_WIA                 0x00    //AK8963的器件ID寄存器地址
    #define MAG_CNTL1               0X0A
    #define MAG_CNTL2               0X0B
    
    #define MAG_XOUT_L              0X03
    #define MAG_XOUT_H              0X04
    #define MAG_YOUT_L              0X05
    #define MAG_YOUT_H              0X06
    #define MAG_ZOUT_L              0X07
    #define MAG_ZOUT_H              0X08
    
    //MPU6500的内部寄存器
    #define MPU_SELF_TESTX_REG      0X0D    //自检寄存器X
    #define MPU_SELF_TESTY_REG      0X0E    //自检寄存器Y
    #define MPU_SELF_TESTZ_REG      0X0F    //自检寄存器Z
    #define MPU_SELF_TESTA_REG      0X10    //自检寄存器A
    #define MPU_SAMPLE_RATE_REG     0X19    //采样频率分频器
    #define MPU_CFG_REG             0X1A    //配置寄存器
    #define MPU_GYRO_CFG_REG        0X1B    //陀螺仪配置寄存器
    #define MPU_ACCEL_CFG_REG       0X1C    //加速度计配置寄存器
    #define MPU_MOTION_DET_REG      0X1F    //运动检测阀值设置寄存器
    #define MPU_FIFO_EN_REG         0X23    //FIFO使能寄存器
    #define MPU_I2CMST_CTRL_REG     0X24    //IIC主机控制寄存器
    #define MPU_I2CSLV0_ADDR_REG    0X25    //IIC从机0器件地址寄存器
    #define MPU_I2CSLV0_REG         0X26    //IIC从机0数据地址寄存器
    #define MPU_I2CSLV0_CTRL_REG    0X27    //IIC从机0控制寄存器
    #define MPU_I2CSLV1_ADDR_REG    0X28    //IIC从机1器件地址寄存器
    #define MPU_I2CSLV1_REG         0X29    //IIC从机1数据地址寄存器
    #define MPU_I2CSLV1_CTRL_REG    0X2A    //IIC从机1控制寄存器
    #define MPU_I2CSLV2_ADDR_REG    0X2B    //IIC从机2器件地址寄存器
    #define MPU_I2CSLV2_REG         0X2C    //IIC从机2数据地址寄存器
    #define MPU_I2CSLV2_CTRL_REG    0X2D    //IIC从机2控制寄存器
    #define MPU_I2CSLV3_ADDR_REG    0X2E    //IIC从机3器件地址寄存器
    #define MPU_I2CSLV3_REG         0X2F    //IIC从机3数据地址寄存器
    #define MPU_I2CSLV3_CTRL_REG    0X30    //IIC从机3控制寄存器
    #define MPU_I2CSLV4_ADDR_REG    0X31    //IIC从机4器件地址寄存器
    #define MPU_I2CSLV4_REG         0X32    //IIC从机4数据地址寄存器
    #define MPU_I2CSLV4_DO_REG      0X33    //IIC从机4写数据寄存器
    #define MPU_I2CSLV4_CTRL_REG    0X34    //IIC从机4控制寄存器
    #define MPU_I2CSLV4_DI_REG      0X35    //IIC从机4读数据寄存器
    
    #define MPU_I2CMST_STA_REG      0X36    //IIC主机状态寄存器
    #define MPU_INTBP_CFG_REG       0X37    //中断/旁路设置寄存器
    #define MPU_INT_EN_REG          0X38    //中断使能寄存器
    #define MPU_INT_STA_REG         0X3A    //中断状态寄存器
    
    #define MPU_ACCEL_XOUTH_REG     0X3B    //加速度值,X轴高8位寄存器
    #define MPU_ACCEL_XOUTL_REG     0X3C    //加速度值,X轴低8位寄存器
    #define MPU_ACCEL_YOUTH_REG     0X3D    //加速度值,Y轴高8位寄存器
    #define MPU_ACCEL_YOUTL_REG     0X3E    //加速度值,Y轴低8位寄存器
    #define MPU_ACCEL_ZOUTH_REG     0X3F    //加速度值,Z轴高8位寄存器
    #define MPU_ACCEL_ZOUTL_REG     0X40    //加速度值,Z轴低8位寄存器
    
    #define MPU_TEMP_OUTH_REG       0X41    //温度值高八位寄存器
    #define MPU_TEMP_OUTL_REG       0X42    //温度值低8位寄存器
    
    #define MPU_GYRO_XOUTH_REG      0X43    //陀螺仪值,X轴高8位寄存器
    #define MPU_GYRO_XOUTL_REG      0X44    //陀螺仪值,X轴低8位寄存器
    #define MPU_GYRO_YOUTH_REG      0X45    //陀螺仪值,Y轴高8位寄存器
    #define MPU_GYRO_YOUTL_REG      0X46    //陀螺仪值,Y轴低8位寄存器
    #define MPU_GYRO_ZOUTH_REG      0X47    //陀螺仪值,Z轴高8位寄存器
    #define MPU_GYRO_ZOUTL_REG      0X48    //陀螺仪值,Z轴低8位寄存器
    
    #define MPU_I2CSLV0_DO_REG      0X63    //IIC从机0数据寄存器
    #define MPU_I2CSLV1_DO_REG      0X64    //IIC从机1数据寄存器
    #define MPU_I2CSLV2_DO_REG      0X65    //IIC从机2数据寄存器
    #define MPU_I2CSLV3_DO_REG      0X66    //IIC从机3数据寄存器
    
    #define MPU_I2CMST_DELAY_REG    0X67    //IIC主机延时管理寄存器
    #define MPU_SIGPATH_RST_REG     0X68    //信号通道复位寄存器
    #define MPU_MDETECT_CTRL_REG    0X69    //运动检测控制寄存器
    #define MPU_USER_CTRL_REG       0X6A    //用户控制寄存器
    #define MPU_PWR_MGMT1_REG       0X6B    //电源管理寄存器1
    #define MPU_PWR_MGMT2_REG       0X6C    //电源管理寄存器2
    #define MPU_FIFO_CNTH_REG       0X72    //FIFO计数寄存器高八位
    #define MPU_FIFO_CNTL_REG       0X73    //FIFO计数寄存器低八位
    #define MPU_FIFO_RW_REG         0X74    //FIFO读写寄存器
    #define MPU_DEVICE_ID_REG       0X75    //器件ID寄存器
    
    #define u8 uint8_t
    #define u16 uint16_t
    
    
    void Read_DMP(void);
    u8 MPU9250_Init(void);
    u8 MPU_WaitForReady(u8 devaddr);
    u8 MPU_Write_Byte(u8 devaddr,u8 reg,u8 data);
    u8 MPU_Read_Byte(u8 devaddr,u8 reg);
    u8 MPU_Set_Gyro_Fsr(u8 fsr);
    u8 MPU_Set_Accel_Fsr(u8 fsr);
    u8 MPU_Set_Rate(u16 rate);
    u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf);
    u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf);
    short MPU_Get_Temperature(void);
    u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz);
    u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az);
    u8 MPU_Get_Magnetometer(short *mx,short *my,short *mz);
    #endif

stm\_陀螺仪mpu9250.cpp主体\_I2C通讯

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "mpu9250.h"
    #include "i2c.h"
    
    void Read_DMP(void)
    {
    /*
     * MPU_Get_Accelerometer(&aacx,&aacy,&aacz);    //得到加速度传感器数据
         ACCEL_X=aacx/164;ACCEL_Y=aacy/164;ACCEL_Z=aacz/(-164);
         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);   //得到陀螺仪数据
       GYRO_X=gyrox/(-16.4);GYRO_Y=gyroy/16.4;GYRO_Z=gyroz/(-16.4);
     * */
    }
    //初始化MPU9250
    //返回值:0,成功
    //    其他,错误代码
    u8 MPU9250_Init(void)
    {
        u8 res=0;
        //IIC_Init();     //初始化IIC总线
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位MPU9250
    
        HAL_Delay(100);  //延时100ms
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU9250
    
        HAL_Delay(100);  //延时100ms
        MPU_Set_Gyro_Fsr(1);                                //陀螺仪传感器,±2000dps
        MPU_Set_Accel_Fsr(0);                               //加速度传感器,±2g
        MPU_Set_Rate(50);                                   //设置采样率50Hz
    
        MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00);   //关闭所有中断
        MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
        MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00);  //关闭FIFO
        MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
        res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG);  //读取MPU6500的ID
        if(res==MPU6500_ID) //器件ID正确
        {
            MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01);    //设置CLKSEL,PLL X轴为参考
            MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00);    //加速度与陀螺仪都工作
            MPU_Set_Rate(50);                               //设置采样率为50Hz
        }else return 1;
    
        res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA);             //读取AK8963 ID
        if(res==AK8963_ID)
        {
            MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11);     //设置AK8963为单次测量模式
        }else return 1;
    
        return 0;
    }
    
    //设置MPU9250陀螺仪传感器满量程范围
    //fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
    //返回值:0,设置成功
    //    其他,设置失败
    u8 MPU_Set_Gyro_Fsr(u8 fsr)
    {
        return MPU_Write_Byte(MPU9250_ADDR,MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
    }
    //设置MPU9250加速度传感器满量程范围
    //fsr:0,±2g;1,±4g;2,±8g;3,±16g
    //返回值:0,设置成功
    //    其他,设置失败
    u8 MPU_Set_Accel_Fsr(u8 fsr)
    {
        return MPU_Write_Byte(MPU9250_ADDR,MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
    }
    
    //设置MPU9250的数字低通滤波器
    //lpf:数字低通滤波频率(Hz)
    //返回值:0,设置成功
    //    其他,设置失败
    u8 MPU_Set_LPF(u16 lpf)
    {
        u8 data=0;
        if(lpf>=188)data=1;
        else if(lpf>=98)data=2;
        else if(lpf>=42)data=3;
        else if(lpf>=20)data=4;
        else if(lpf>=10)data=5;
        else data=6;
        return MPU_Write_Byte(MPU9250_ADDR,MPU_CFG_REG,data);//设置数字低通滤波器
    }
    
    //设置MPU9250的采样率(假定Fs=1KHz)
    //rate:4~1000(Hz)
    //返回值:0,设置成功
    //    其他,设置失败
    u8 MPU_Set_Rate(u16 rate)
    {
        u8 data;
        if(rate>1000)rate=1000;
        if(rate<4)rate=4;
        data=1000/rate-1;
        data=MPU_Write_Byte(MPU9250_ADDR,MPU_SAMPLE_RATE_REG,data); //设置数字低通滤波器
        return MPU_Set_LPF(rate/2); //自动设置LPF为采样率的一半
    }
    
    //得到温度值
    //返回值:温度值(扩大了100倍)
    short MPU_Get_Temperature(void)
    {
        u8 buf[2];
        short raw;
        float temp;
        MPU_Read_Len(MPU9250_ADDR,MPU_TEMP_OUTH_REG,2,buf);
        raw=((u16)buf[0]<<8)|buf[1];
        temp=21+((double)raw)/333.87;
        return temp*100;;
    }
    //得到陀螺仪值(原始值)
    //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
    //返回值:0,成功
    //    其他,错误代码
    u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
    {
        u8 buf[6],res;
        res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
        if(res==0)
        {
            *gx=((u16)buf[2]<<8)|buf[3];
            *gy=((u16)buf[0]<<8)|buf[1];
            *gz=((u16)buf[4]<<8)|buf[5];
        }
        return res;;
    }
    //得到加速度值(原始值)
    //gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
    //返回值:0,成功
    //    其他,错误代码
    u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
    {
        u8 buf[6],res;
        res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
        if(res==0)
        {
            *ax=((u16)buf[0]<<8)|buf[1];
            *ay=((u16)buf[2]<<8)|buf[3];
            *az=((u16)buf[4]<<8)|buf[5];
        }
        return res;;
    }
    
    //得到磁力计值(原始值)
    //mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
    //返回值:0,成功
    //    其他,错误代码
    u8 MPU_Get_Magnetometer(short *mx,short *my,short *mz)
    {
        u8 buf[6],res;
        res=MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,buf);
        if(res==0)
        {
            *mx=((u16)buf[1]<<8)|buf[0];
            *my=((u16)buf[3]<<8)|buf[2];
            *mz=((u16)buf[5]<<8)|buf[4];
        }
        MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //AK8963每次读完以后都需要重新设置为单次测量模式
        return res;;
    }
    
    //IIC连续写
    //addr:器件地址
    //reg:寄存器地址
    //len:写入长度
    //buf:数据区
    //返回值:0,正常
    //    其他,错误代码
    u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
    {
        HAL_I2C_Mem_Write(&hi2c1,addr<<1,reg,I2C_MEMADD_SIZE_8BIT,buf,len,300);
        /* u8 i;
         IIC_Start();
         IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
         if(IIC_Wait_Ack())          //等待应答
         {
             IIC_Stop();
             return 1;
         }
         IIC_Send_Byte(reg);         //写寄存器地址
         IIC_Wait_Ack();             //等待应答
         for(i=0;i<len;i++)
         {
             IIC_Send_Byte(buf[i]);  //发送数据
             if(IIC_Wait_Ack())      //等待ACK
             {
                 IIC_Stop();
                 return 1;
             }
         }
         IIC_Stop();*/
        return 0;
    }
    
    //IIC连续读
    //addr:器件地址
    //reg:要读取的寄存器地址
    //len:要读取的长度
    //buf:读取到的数据存储区
    //返回值:0,正常
    //    其他,错误代码
    u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
    {
        HAL_I2C_Mem_Read(&hi2c1, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, buf, len, 300);
        /*IIC_Start();
        IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
        if(IIC_Wait_Ack())          //等待应答
        {
            IIC_Stop();
            return 1;
        }
        IIC_Send_Byte(reg);         //写寄存器地址
        IIC_Wait_Ack();             //等待应答
        IIC_Start();
        IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
        IIC_Wait_Ack();             //等待应答
        while(len)
        {
            if(len==1)*buf=IIC_Read_Byte(0);//读数据,发送nACK
            else *buf=IIC_Read_Byte(1);     //读数据,发送ACK
            len--;
            buf++;
        }
        IIC_Stop();                 //产生一个停止条件*/
        return 0;
    }
    
    //IIC写一个字节
    //devaddr:器件IIC地址
    //reg:寄存器地址
    //data:数据
    //返回值:0,正常
    //    其他,错误代码
    u8 MPU_Write_Byte(u8 addr,u8 reg,u8 data)
    {
        HAL_I2C_Mem_Write(&hi2c1,addr<<1,reg,I2C_MEMADD_SIZE_8BIT,&data,1,300);
    //    IIC_Start();
        IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
        if(IIC_Wait_Ack())          //等待应答
        {
            IIC_Stop();
            return 1;
        }
        IIC_Send_Byte(reg);         //写寄存器地址
        IIC_Wait_Ack();             //等待应答
        IIC_Send_Byte(data);        //发送数据
        if(IIC_Wait_Ack())          //等待ACK
        {
            IIC_Stop();
            return 1;
        }
        IIC_Stop();
        return 0;
    }
    
    //IIC读一个字节
    //reg:寄存器地址
    //返回值:读到的数据
    u8 MPU_Read_Byte(u8 addr,u8 reg)
    {
        uint8_t res;
        HAL_I2C_Mem_Read(&hi2c1, addr<<1, reg, I2C_MEMADD_SIZE_8BIT, &res, 1, 300);
    //    u8 res;
    //    IIC_Start();
    //    IIC_Send_Byte((addr<<1)|0); //发送器件地址+写命令
    //    IIC_Wait_Ack();             //等待应答
    //    IIC_Send_Byte(reg);         //写寄存器地址
    //    IIC_Wait_Ack();             //等待应答
    //    IIC_Start();
    //    IIC_Send_Byte((addr<<1)|1); //发送器件地址+读命令
    //    IIC_Wait_Ack();             //等待应答
    //    res=IIC_Read_Byte(0);     //读数据,发送nACK
    //    IIC_Stop();                 //产生一个停止条件
    
        return res;
    }

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 14][]

stm\_编码器获取轮子速度\_m/s

/*    vel_update_time速度的更新时间;  单位时间内测量轮子的转速: 每隔1000/MOVE_CTRL_RATE 计算一次轮子的转速
          *    WHEEL_TPR  (13 * 30 * 4) 编码器13个孔,变速比30
          *    WHEEL_DIAMETER: 轮子的直径 0.064f
          *    MOVE_CTRL_RATE 100 编码器单位时间 1秒执行100次
           */
        if(HAL_GetTick() - vel_update_time > (1000/MOVE_CTRL_RATE)){
            //  速度的更新的时间
            vel_update_time = HAL_GetTick();
    
            // 读取编码器当前的数据
            short count = this->encoder->read();
            /**
             * 计算速度 m/s:
             *      count/一圈的数量 => 圈数
             *  圈数转成距离:   圈数 * 轮子的周长 =  圈数 * ( 3.14 * 轮子的直径)  ==> m  D
             *  单位时间:   1000/MOVE_CTRL_RATE ms
             *  速度:    D/单位时间 ===>  D/1000/MOVE_CTRL_RATE  m/ms ===> m/s
             *                           D*MOVE_CTRL_RATE/1000 ===> D*MOVE_CTRL_RATE
             *         圈数 * ( 3.14 * 轮子的直径)*MOVE_CTRL_RATE
             *         count/一圈的数量 * ( 3.14 * 轮子的直径)*MOVE_CTRL_RATE
             *         WHEEL_TPR  (13 * 30 * 4) 编码器13个孔,变速比30
             *         WHEEL_DIAMETER: 轮子的直径 0.064f
             *         MOVE_CTRL_RATE 100 编码器单位时间 1秒执行100次
             */
            // vel= 速度 m/s
            vel = (float)count/WHEEL_TPR*PI*WHEEL_DIAMETER*MOVE_CTRL_RATE;
    
          
        }

stm\_轮子差速\_计算小车的每个轮子输出的速度,控制小车\_前后\_左右

/**
     * 更新小车速度, 并且分别计算每个轮子的转速
     * @param xVel x方向速度
     * @param yVel y方向速度
     * @param angularVel 角速度
     */
    void updateVel(float xVel,float yVel,float angularVel){
          /*
         * WHEEL_DISTANCE 左右轮子的距离;   前后轮子的轴距 WHEEL_AXIS
         * 线速度v = 角速度w 乘以 转弯半径R (小车中心点),d: 轮子到小车中心的距离;
         * 小车中心的线速度 V=w*R
         * 左轮线速度 v1=w*(R-d)   右轮线速度: v2=w*(R-d);
         * WHEEL_DISTANCE=2d
         * 左轮线速度 v1=w*(R-WHEEL_DISTANCE*0.5)   右轮线速度: v2=w*(R-dWHEEL_DISTANCE*0.5);
         * v1=w*R- w*WHEEL_DISTANCE*0.5=V-w*WHEEL_DISTANCE*0.5 ; d= WHEEL_DISTANCE*0.5  [v1=V-wd]
         * v2=w*R+ w*WHEEL_DISTANCE*0.5=V+w*WHEEL_DISTANCE*0.5 ; d= WHEEL_DISTANCE*0.5  [v2=V+wd]
         * 
         * 四个轮子
         * 左边轮速度 vl=V-w(d+c); d= WHEEL_DISTANCE*0.5  c=WHEEL_AXIS*0.5
         * 右边轮速度 vr=V+w(d+c); d= WHEEL_DISTANCE*0.5  c=WHEEL_AXIS*0.5
         */
        float wd = angularVel*(WHEEL_DISTANCE+WHEEL_AXIS)*0.5;
        float vel1 = xVel - yVel - wd;
        float vel3 = xVel + yVel - wd;
        float vel2 = xVel + yVel + wd;
        float vel4 = xVel - yVel + wd;
    
        wheel1->setVel(vel1);
        wheel2->setVel(vel2);
        wheel3->setVel(vel3);
        wheel4->setVel(vel4);
    
    }

stm\_pid算法得到方波

float kp=25;
     float ki=0.1;
     float kd=0.5;
        //方波
        float pwm;
       // 上一次的误差
        float prevError;
        // 积分
        float intergral;
        // 微分
        float derivative;
    
    /**
        * pid的计算函数
        * @param target  目标值
        * @param current 当前值
        * @return  pwm
        */
    float compute(float target, float current) {
        // 计算误差
        float error = target - current;
        // 误差的累计
        intergral +=error;
        // 本次误差和上一次误差的差异
        derivative = error - prevError;
    
        // 套用pid的公式
        pwm += kp*error + ki*intergral + kd*derivative;
        // 记录上一次的误差
        prevError = error;
    
        return pwm;
    }

stm\_Car.cpp主体\_小车\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "Car.h"
    #include "config.h"
    
    /**
     * 有一台车,四个轮子,每个轮子包含一个电机和一个编码器
     * 车的对象:Car
     *            输入目标速度
     *            输出当前速度
     * 轮子的对象: Wheel
     *        电机: Motor  : 控制电机转动
     *        编码器: Encoder : 读取编码器的数据
     *  Car
     *     Wheel
     *         motor
     *         encoder
     *
     */
    Car::Car() {
        //初始化1号电机
        Motor* motor1 = new Motor(MOTOR1_A_GPIO_PORT,MOTOR1_A_PIN,MOTOR1_B_GPIO_PORT,MOTOR1_B_PIN,MOTOR1_TIM,MOTOR1_CHANNEL,MOTOR1_DIRECTION);
        //初始化1号编码器
        Encoder* encoder1 = new Encoder(ENCODER1_TIM,ENCODER1_CHANNEL,ENCODER1_DIRECTION);
        // 初始化1号轮子
        wheel1 = new Wheel(motor1,encoder1);
    
        //初始化2号电机
        Motor* motor2 = new Motor(MOTOR2_A_GPIO_PORT,MOTOR2_A_PIN,MOTOR2_B_GPIO_PORT,MOTOR2_B_PIN,MOTOR2_TIM,MOTOR2_CHANNEL,MOTOR2_DIRECTION);
        //初始化2号编码器
        Encoder* encoder2 = new Encoder(ENCODER2_TIM,ENCODER2_CHANNEL,ENCODER2_DIRECTION);
        // 初始化2号轮子
        wheel2 = new Wheel(motor2,encoder2);
    
        //初始化3号电机
        Motor* motor3 = new Motor(MOTOR3_A_GPIO_PORT,MOTOR3_A_PIN,MOTOR3_B_GPIO_PORT,MOTOR3_B_PIN,MOTOR3_TIM,MOTOR3_CHANNEL,MOTOR3_DIRECTION);
        //初始化3号编码器
        Encoder* encoder3 = new Encoder(ENCODER3_TIM,ENCODER3_CHANNEL,ENCODER3_DIRECTION);
        // 初始化3号轮子
        wheel3 = new Wheel(motor3,encoder3);
    
        //初始化3号电机
        Motor* motor4 = new Motor(MOTOR4_A_GPIO_PORT,MOTOR4_A_PIN,MOTOR4_B_GPIO_PORT,MOTOR4_B_PIN,MOTOR4_TIM,MOTOR4_CHANNEL,MOTOR4_DIRECTION);
        //初始化3号编码器
        Encoder* encoder4 = new Encoder(ENCODER4_TIM,ENCODER4_CHANNEL,ENCODER4_DIRECTION);
        // 初始化3号轮子
        wheel4 = new Wheel(motor4,encoder4);
    
    }
    
    void Car::init(){
        wheel1->init();
        wheel2->init();
        wheel3->init();
        wheel4->init();
    
        wheel1->setVel(0.001);
        wheel2->setVel(0.001);
        wheel3->setVel(0.001);
        wheel4->setVel(0.001);
    }
    /**
     * 更新小车速度, 并且分别计算每个轮子的转速
     * @param xVel x方向速度
     * @param yVel y方向速度
     * @param angularVel 角速度
     */
    void Car::updateVel(float xVel,float yVel,float angularVel){
    
        /*
         * WHEEL_DISTANCE 左右轮子的距离;   前后轮子的轴距 WHEEL_AXIS
         * 线速度v = 角速度w 乘以 转弯半径R (小车中心点),d: 轮子到小车中心的距离;
         * 小车中心的线速度 V=w*R
         * 左轮线速度 v1=w*(R-d)   右轮线速度: v2=w*(R-d);
         * WHEEL_DISTANCE=2d
         * 左轮线速度 v1=w*(R-WHEEL_DISTANCE*0.5)   右轮线速度: v2=w*(R-dWHEEL_DISTANCE*0.5);
         * v1=w*R- w*WHEEL_DISTANCE*0.5=V-w*WHEEL_DISTANCE*0.5 ; d= WHEEL_DISTANCE*0.5  [v1=V-wd]
         * v2=w*R+ w*WHEEL_DISTANCE*0.5=V+w*WHEEL_DISTANCE*0.5 ; d= WHEEL_DISTANCE*0.5  [v2=V+wd]
         *
         * 四个轮子
         * 左边轮速度 vl=V-w(d+c); d= WHEEL_DISTANCE*0.5  c=WHEEL_AXIS*0.5
         * 右边轮速度 vr=V+w(d+c); d= WHEEL_DISTANCE*0.5  c=WHEEL_AXIS*0.5
         */
        float wd = angularVel*(WHEEL_DISTANCE+WHEEL_AXIS)*0.5;
        float vel1 = xVel - yVel - wd;
        float vel3 = xVel + yVel - wd;
        float vel2 = xVel + yVel + wd;
        float vel4 = xVel - yVel + wd;
    
        wheel1->setVel(vel1);
        wheel2->setVel(vel2);
        wheel3->setVel(vel3);
        wheel4->setVel(vel4);
    
    }
    
    void Car::tick(){
        wheel1->tick();
        wheel2->tick();
        wheel3->tick();
        wheel4->tick();
    }
    
    
    
    /*
     *  已知左右轮的速度,我们推导当前小车前进的线速度和角速度
     */
    float Car::getXVel(){
        float vel1 = wheel1->vel;
        float vel2 = wheel2->vel;
        float vel3 = wheel3->vel;
        float vel4 = wheel4->vel;
        //转换成线速度
        return (vel1+vel2+vel3+vel4)/4;
    }
    
    float Car::getYVel(){
        return 0;
    }
    /*
     *  已知左右轮的速度,我们推导当前小车前进的线速度和角速度
     */
    float Car::getAngularVel(){
        float vel1 = wheel1->vel;
        float vel2 = wheel2->vel;
        float vel3 = wheel3->vel;
        float vel4 = wheel4->vel;
        //转换成角速度
        float angular = (-vel1-vel3 + vel2 + vel4)/4/((WHEEL_AXIS+WHEEL_DISTANCE)*0.5);
        return angular;
    }

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 15][]

stm\_Car.h头\_小车\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_CAR_H
    #define HEIMAROBOTV4_CAR_H
    
    #include "Wheel.h"
    
    class Car {
    public:
        Car();
        //车轮1
        Wheel* wheel1;
        //车轮2
        Wheel* wheel2;
        //车轮3
        Wheel* wheel3;
        //车轮4
        Wheel* wheel4;
    
    public:
        /*
         * 小车初始化
         */
        void init();
        /*
         * 小车心跳
         */
        void tick();
        /*
         * 更新线速度 与角速度
         * xVel:x方向速度
         * yVel:y方向速度
         * angularVel:角速度
         */
        void updateVel(float xVel,float yVel,float angularVel);
        /*
         * 获取 x方向速度
         */
        float getXVel();
        /*
         * 获取 y方向速度
         */
        float getYVel();
        /*
         * 获取 角速度
         */
        float getAngularVel();
    
    
    
    };
    
    
    #endif //HEIMAROBOTV4_CAR_H

stm\_Wheel.cpp主体\_轮子\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "Wheel.h"
    #include "config.h"
    #include <cstdio>
    
    Wheel::Wheel(Motor* motor, Encoder* encoder) {
        //电机
        this->motor = motor;
        //编码器
        this->encoder = encoder;
        this->pid = new PID(150,0,150);
        //目标速度
        this->targetVel = 0;
    }
    
    void Wheel::init(){
        //编码器初始化
        this->encoder->init();
        //电机初始化
        this->motor->init();
    }
    void Wheel::tick(){
         /*    vel_update_time速度的更新时间;  单位时间内测量轮子的转速: 每隔1000/MOVE_CTRL_RATE 计算一次轮子的转速
          *    WHEEL_TPR  (13 * 30 * 4) 编码器13个孔,变速比30
          *    WHEEL_DIAMETER: 轮子的直径 0.064f
          *    MOVE_CTRL_RATE 100 编码器单位时间 1秒执行100次
           */
        if(HAL_GetTick() - vel_update_time > (1000/MOVE_CTRL_RATE)){
            //  速度的更新的时间
            vel_update_time = HAL_GetTick();
    
            // 读取编码器当前的数据
            short count = this->encoder->read();
            /**
             * 计算速度 m/s:
             *      count/一圈的数量 => 圈数
             *  圈数转成距离:   圈数 * 轮子的周长 =  圈数 * ( 3.14 * 轮子的直径)  ==> m  D
             *  单位时间:   1000/MOVE_CTRL_RATE ms
             *  速度:    D/单位时间 ===>  D/1000/MOVE_CTRL_RATE  m/ms ===> m/s
             *                           D*MOVE_CTRL_RATE/1000 ===> D*MOVE_CTRL_RATE
             *         圈数 * ( 3.14 * 轮子的直径)*MOVE_CTRL_RATE
             *         count/一圈的数量 * ( 3.14 * 轮子的直径)*MOVE_CTRL_RATE
             *         WHEEL_TPR  (13 * 30 * 4) 编码器13个孔,变速比30
             *         WHEEL_DIAMETER: 轮子的直径 0.064f
             *         MOVE_CTRL_RATE 100 编码器单位时间 1秒执行100次
             */
            // vel= 速度 m/s
            vel = (float)count/WHEEL_TPR*PI*WHEEL_DIAMETER*MOVE_CTRL_RATE;
    
            /*
             * pid 控制逻辑
             * this->targetVel 目标速度
             * this->vel 当前速度
             * */
            float pwm = this->pid->compute(this->targetVel,this->vel);
            printf("vel=%f tar=%f  count=%d pwm=%f \r\n",vel,targetVel,count,pwm);
            this->motor->spin(pwm);
        }
    }
    /**
         * 设置轮子的转速
         * @param vel  单位m/s
         */
    void Wheel::setVel(float vel){
        // 要让轮子转动到指定的目标速度: PID  闭环控制, 轮子当前的速度
        if(vel != this->targetVel){
            this->pid->reset();
        }
    
        this->targetVel = vel;
    }
    //获取轮子当前的转速
    float Wheel::getVel(){
        return 0;
    }
    
    Wheel::~Wheel() {
        delete this->motor;
        delete this->encoder;
        delete this->pid;
    }

stm\_Wheel.h头\_轮子\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_WHEEL_H
    #define HEIMAROBOTV4_WHEEL_H
    
    #include "Encoder.h"
    #include "Motor.h"
    #include "PID.h"
    /*
     * 轮子
     */
    class Wheel {
    public:
        /*
         * motor 电机
         * encoder 编码器
         */
        Wheel(Motor* motor,Encoder* encoder);
        ~Wheel();
        //电机
        Motor* motor;
        //编码器
        Encoder* encoder;
        PID* pid;
        // 速度的更新的时间   vel_update_time = HAL_GetTick();
        uint32_t vel_update_time;
        // 速度 m/s
        float vel;
        //
        float targetVel;
    public:
        /*
         * 轮子初始化
         */
        void init();
        /**
         * 轮子心跳
         */
        void tick();
        /**
         * 设置轮子的转速
         * @param vel  单位m/s
         */
        void setVel(float vel);
        /*
         * 获取轮子当前的速度
         */
        float getVel();
    };
    
    
    #endif //HEIMAROBOTV4_WHEEL_H

stm_Motor.cpp主体_电机_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "Motor.h"
    #include <math.h>
    #include "config.h"
    
    Motor::Motor(GPIO_TypeDef *portA, uint16_t pinA,GPIO_TypeDef *portB, uint16_t pinB, TIM_HandleTypeDef *tim,uint16_t channel,int direction){
        this->portA = portA;
        this->pinA = pinA;
        this->portB = portB;
        this->pinB = pinB;
        this->tim = tim;
        this->channel = channel;
        this->direction = direction;
    }
    
    void Motor::init() {
        HAL_GPIO_WritePin(this->portA,this->pinA,GPIO_PIN_RESET);
        HAL_GPIO_WritePin(this->portB,this->pinB,GPIO_PIN_RESET);
    //    HAL_TIM_PWM_Stop(this->tim,this->channel);
        //1.控制电机转速启动定时器 开始计数 在init中开启
        HAL_TIM_PWM_Start(this->tim,this->channel);
    // //1.启动定时器 开始计数   __HAL_TIM_SET_COUNTER(this->tim,0);
        __HAL_TIM_SET_COMPARE(this->tim,this->channel,0);
    }
    /**
     * 控制电机转动
     * @param pwm
     */
    void Motor::spin(int pwm) {
       //1.控制电机转速启动定时器 开始计数 在init中开启
      // HAL_TIM_PWM_Start(this->tim,this->channel);
    
        // pwm限制范围
        if(pwm>MAX_PWM){
            pwm = MAX_PWM;
        }else if(pwm < MIN_PWM){
            pwm = MIN_PWM;
        }
    
        pwm*=this->direction;
    
        if(pwm>0){ // 正转
            HAL_GPIO_WritePin(this->portA,this->pinA,GPIO_PIN_SET);
            HAL_GPIO_WritePin(this->portB,this->pinB,GPIO_PIN_RESET);
        }else if(pwm < 0){
            HAL_GPIO_WritePin(this->portA,this->pinA,GPIO_PIN_RESET);
            HAL_GPIO_WritePin(this->portB,this->pinB,GPIO_PIN_SET);
        }else{
            HAL_GPIO_WritePin(this->portA,this->pinA,GPIO_PIN_RESET);
            HAL_GPIO_WritePin(this->portB,this->pinB,GPIO_PIN_RESET);
        }
       //设置预值,计数器的比较值  参数3: pwm 设置 7200是一直高电平, 参数3:pwm 设置 3600是高电平,7200-3600 是低点平  参数3:pwm 设置 0一直低电平
        __HAL_TIM_SET_COMPARE(this->tim,this->channel,abs(pwm));
    }

stm\_Motor.h头\_电机\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_MOTOR_H
    #define HEIMAROBOTV4_MOTOR_H
    
    #include "stm32f1xx_hal.h"
    /*
     * 电机
     */
    class Motor {
    public:
        /**
         * 电机
         * @param portA  控制正反转
         * @param pinA 控制正反转
         * @param portB 控制正反转
         * @param pinB 控制正反转
         * @param tim   控制PWM的
         * @param channel   定时器通道
         * @param direction  后续方便调整电机的转动方向
         */
        Motor(GPIO_TypeDef *portA, uint16_t pinA,GPIO_TypeDef *portB, uint16_t pinB, TIM_HandleTypeDef *tim,uint16_t channel,int direction);
        // A_PORT A_PIN
        GPIO_TypeDef *portA;
        uint16_t pinA;
        // B_PORT B_PIN
        GPIO_TypeDef *portB;
        uint16_t pinB;
        // TIM 用的定时器
        TIM_HandleTypeDef *tim;
    
        // CHANNEL 通道
        uint16_t channel;
        //后续方便调整电机的转动方向
        int direction;
    
        //让电机转起来 接收方波
        void spin(int pwm);
        void init();
    //    void tick();
    };
    
    
    #endif //HEIMAROBOTV4_MOTOR_H

stm\_Encoder.cpp主体\_编码器\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #include "Encoder.h"
    Encoder::Encoder(TIM_HandleTypeDef *tim,uint16_t channel,int direction){
        this->tim = tim;
        this->channel = channel;
        //控制编码器正反方向
        this->direction = direction;
    }
    
    void Encoder::init(){
        // 启动定时器
        HAL_TIM_Encoder_Start(this->tim,this->channel);
    }
    
    short Encoder::read(){
        // 读取编码器数据
        short count = __HAL_TIM_GET_COUNTER(this->tim);
        // 通过direction调整编码器的方向
        count *= this->direction;
        // 读完之后清空数据
        __HAL_TIM_SET_COUNTER(this->tim,0);
        return count;
    }

stm\_Encoder.h头\_编码器\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_ENCODER_H
    #define HEIMAROBOTV4_ENCODER_H
    
    #include "stm32f1xx_hal.h"
    /**
     * 电机编码器
     */
    class Encoder {
    public:
        Encoder(TIM_HandleTypeDef *tim,uint16_t channel,int direction);
    
        TIM_HandleTypeDef *tim;
        uint16_t channel;
        int direction;
        /*
         * 初始化
         */
        void init();
        /**
         * 读取编码器的值
         */
        short read();
    };
    
    
    #endif //HEIMAROBOTV4_ENCODER_H

stm_PID.cpp主体_11

//
    // Created by Kaijun on 2020/9/11.
    //
    #include "PID.h"
    #include <cstdio>
    
    PID::PID(float kp, float ki, float kd) {
        this->kp = kp;
        this->ki = ki;
        this->kd = kd;
    }
    /**
        * pid的计算函数
        * @param target  目标值
        * @param current 当前值
        * @return  pwm
        */
    float PID::compute(float target, float current) {
        // 计算误差
        float error = target - current;
        // 误差的累计
        intergral +=error;
        // 本次误差和上一次误差的差异
        derivative = error - prevError;
    
        // 套用pid的公式
        pwm += kp*error + ki*intergral + kd*derivative;
        // 记录上一次的误差
        prevError = error;
    
        return pwm;
    }
    
    void PID::reset(){
        pwm = 0;
        intergral = 0;
        derivative = 0;
        prevError = 0;
    }

stm\_PID.h头\_11

//
    // Created by Kaijun on 2020/9/11.
    //
    
    #ifndef HEIMAROBOTV4_PID_H
    #define HEIMAROBOTV4_PID_H
    
    
    class PID {
    public:
        PID(float kp,float ki,float kd);
        float kp;
        float ki;
        float kd;
        //方波
        float pwm;
        // 上一次的误差
        float prevError;
        // 积分
        float intergral;
        // 微分
        float derivative;
        /**
         * pid的计算函数
         * @param target  目标值
         * @param current 当前值
         * @return  pwm
         */
        float compute(float target,float current);
        /**
         *  重置所有的误差: 当设置的速度 和 上一次不一样
         */
        void reset();
    };
    
    
    #endif //HEIMAROBOTV4_PID_H

stm\_Car.h.cpp\_启动调用\_11

//初始化操作
    car.init();
    
    car.tick();
    
    void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            uint8_t key = buffer[0];
    
            switch (key){
                case 0x47:
                    car.updateVel(0.2,0,0);
                    break;
                case 0x48:
                    car.updateVel(0,0,0.8);
                    break;
                case 0x49:
                    car.updateVel(0,0,0);
                    break;
                case 0x4A:
                    car.updateVel(0,0,-0.8);
                    break;
                case 0x4B:
                    car.updateVel(-0.2,0,0);
                    break;
    
                // 麦克娜姆轮
                case 0x41:
                    car.updateVel(0.2,0.2,0); // 左前方
                    break;
                case 0x42:
                    car.updateVel(0,0.2,0); // 水平向左
                    break;
                case 0x43:
                    car.updateVel(0.2,-0.2,0); // 右前方
                    break;
                case 0x44:
                    car.updateVel(-0.2,0.2,0); // 左后方
                    break;
                case 0x45:
                    car.updateVel(0,-0.2,0); // 水平向右
                    break;
                case 0x46:
                    car.updateVel(-0.2,-0.2,0); // 右后方
                    break;
            }
            HAL_UART_Receive_IT(&huart2,buffer,1);
        }
    }

stm\_HAL\_UART\_RxCpltCallback中断\_遥控\_发送遥控

void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
    {
    
        /*char data[]="callback";
        uint16_t len = sizeof(data)-1;
        // 1. 串口对象, 2.发送的数据, 3.数据的长度, 4. 超时等待时间
        HAL_UART_Transmit(&huart1, (uint8_t *)(data), len, 3000);*/
    
      //  printf("sssss \r\n");
    
    // 判断串口1的中断处理逻辑
        if(huart->Instance == USART1){
            char key = buffer2[0];
            //printf(reinterpret_cast<const char *>(key));
    
           // printf("1 \r\n");
            switch (key){
                //case 0x47:
                case 'a':
                    car.updateVel(0.2,0,0);
                    printf("===aa== \r\n");
                    break;
                case 'b':
                    car.updateVel(0,0,0.8);
                    printf("===bb== \r\n");
                    break;
              //  case 0x49:
                case 'c':
                    car.updateVel(0,0,0);
                    printf("===cc== \r\n");
                    break;
                case 'd':
                    car.updateVel(0,0,-0.8);
                    printf("===dd== \r\n");
                    break;
                case 'e':
                    car.updateVel(-0.2,0,0);
                    printf("===ee== \r\n");
                    break;
    
                    // 麦克娜姆轮
                case 0x41:
                    car.updateVel(0.2,0.2,0); // 左前方
                    break;
                case 0x42:
                    car.updateVel(0,0.2,0); // 水平向左
                    break;
                case 0x43:
                    car.updateVel(0.2,-0.2,0); // 右前方
                    break;
                case 0x44:
                    car.updateVel(-0.2,0.2,0); // 左后方
                    break;
                case 0x45:
                    car.updateVel(0,-0.2,0); // 水平向右
                    break;
                case 0x46:
                    car.updateVel(-0.2,-0.2,0); // 右后方
                    break;
    
                default:
                    printf("==============");
    
            }
            // 如果想让中断连续可用,需要重新开启中断
            HAL_UART_Receive_IT(&huart1,buffer2,1);
        }
        // 判断串口1的中断处理逻辑
        if(huart->Instance == USART2){
            // 切换led灯的状态
            HAL_GPIO_TogglePin(GPIOE,GPIO_PIN_10);
            uint8_t key = buffer[0];
    
            switch (key){
                case 0x47:
                    car.updateVel(0.2,0,0);
                    break;
                case 0x48:
                    car.updateVel(0,0,0.8);
                    break;
                case 0x49:
                    car.updateVel(0,0,0);
                    break;
                case 0x4A:
                    car.updateVel(0,0,-0.8);
                    break;
                case 0x4B:
                    car.updateVel(-0.2,0,0);
                    break;
    
                // 麦克娜姆轮
                case 0x41:
                    car.updateVel(0.2,0.2,0); // 左前方
                    break;
                case 0x42:
                    car.updateVel(0,0.2,0); // 水平向左
                    break;
                case 0x43:
                    car.updateVel(0.2,-0.2,0); // 右前方
                    break;
                case 0x44:
                    car.updateVel(-0.2,0.2,0); // 左后方
                    break;
                case 0x45:
                    car.updateVel(0,-0.2,0); // 水平向右
                    break;
                case 0x46:
                    car.updateVel(-0.2,-0.2,0); // 右后方
                    break;
            }
            HAL_UART_Receive_IT(&huart2,buffer,1);
        }
    }

stm\_protocol.h\_car\_把速度\_角度\_陀螺仪数据封装在结构体中

//
    // Created by Kaijun on 2020/9/13.
    //
    
    #ifndef HEIMAROBOTV4_PROTOCOL_H
    #define HEIMAROBOTV4_PROTOCOL_H
    
    #include <stdint.h>
    #define FLAG_HEAD0 0xce
    #define FLAG_HEAD1 0xfa
    #define FLAG_TAIL  0xad
    
    //CE FA 00 00 4B 0E D8 02 C8 FE 58
    //3C E9 FF 91 00 8A 00 00 91 FF E9
    //00 8A F4 01 20 03 AD
    
    
    #pragma pack(1)
    struct TXProtocol{
        uint8_t head0;
        uint8_t head1;
        uint8_t type;
        uint8_t len;
    
        short temperature;
    
        short ax;
        short ay;
        short az;
    
        short gx;
        short gy;
        short gz;
    
        short mx;
        short my;
        short mz;
    
        short velocity;
        short angular;
    //    uint8_t tail;
    };
    
    
    // 发送的数据协议 // 让数据按1字节进行对齐
    /*#pragma pack(1)
    typedef struct TXProtocol{
        // 使用两个帧头不那么容易出错
        uint8_t head0;
        uint8_t head1;
        // 定义数据帧的类型: 0x01 速度信息  0x02 陀螺仪信息 0x03 所有信息
        uint8_t type;
        // 数据的长度
        uint8_t length;
        // 温度
        short temperature;
        // 加速度
        short ax;
        short ay;
        short az;
        // 角速度
        short gx;
        short gy;
        short gz;
        // 地磁
        short mx;
        short my;
        short mz;
        // 速度信息
        short velocity;
        short angular;
    
    //    uint8_t tail;
        // 29个字节
    };*/
    
    
    #endif //HEIMAROBOTV4_PROTOCOL_H

stm\_ \#pragma\_让数据按1字节进行对齐

cpp\_ \#pragma\_让数据按1字节进行对齐

// #pragma pack(1) 让数据按1字节进行对齐
    #pragma pack(1)

stm\_ struct \_定义结构体

cpp\_ struct \_定义结构体

// #pragma pack(1) 让数据按1字节进行对齐
    //#pragma pack(1)
    typedef struct TXProtocol{
        // 使用两个帧头不那么容易出错
        uint8_t head0;
        // 温度
        short temperature;
      
    };
    
    
    
    //使用
    struct TXProtocol* protocol = new TXProtocol;
        protocol->head0 = FLAG_HEAD0;
        // 温度
        protocol->temperature = temp;
    
        //  将数据发送出去
       // common_uart_send((uint8_t*)protocol, sizeof(TXProtocol));
    
        // 切记 new 出来的东西一定要删除
        delete protocol;

stm_common_uart.h头_DMA传输_2

//
    // Created by Kaijun on 2020/9/13.
    //
    
    //
    // Created by Kaijun on 2020/6/8.
    //
    
    #ifndef HEIMAROBOT4WD_COMMON_UART_H
    #define HEIMAROBOT4WD_COMMON_UART_H
    
    #ifdef __cplusplus
    extern "C" {
    #endif
    #include "usart.h"
    #include "stm32f1xx_hal.h"
    
    
    extern DMA_HandleTypeDef hdma_usart1_rx;
    extern UART_HandleTypeDef huart1;
    
    // 初始化
    void common_uart_init();
    
    void common_uart_idle_handle(UART_HandleTypeDef *huart1);
    
    void common_uart_send(uint8_t *data, uint16_t size);
    
    __weak void common_uart_idle_callback(uint8_t receive_buf[],uint16_t receive_len);
    
    
    #ifdef __cplusplus
    }
    #endif
    
    #endif //HEIMAROBOT4WD_COMMON_UART_H

stm\_common\_uart.cpp主体\_DMA传输\_2

//
    // Created by Kaijun on 2020/9/13.
    //
    
    #include "common_uart.h"
    //
    // Created by Kaijun on 2020/6/8.
    //
    #include "common_uart.h"
    #include <string.h>
    #include <stdio.h>
    #include "stm32f1xx_hal.h"
    
    // 定义缓冲区的大小
    const uint32_t BUFFER_SIZE = 255;
    // 声明缓冲区
    static uint8_t uart_rx_buff[255];
    /**
     * 串口的初始化操作
     */
    void common_uart_init(){
        // 开启DMA接收数据
        HAL_UART_Receive_DMA(&huart1, (uint8_t*)uart_rx_buff, BUFFER_SIZE);
        // 开启空闲中断处理
        __HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);
        // 开启通信异常处理
        __HAL_UART_ENABLE_IT(&huart1,UART_IT_ERR);
    }
    
    /**
     * 需要将这个函数放到stm32f1xx_it.c 文件中的void USART1_IRQHandler(void)函数中调用
     * @param huart1
     */
    void common_uart_idle_handle(UART_HandleTypeDef* huart1){
    
        if(USART1 == huart1->Instance)
        {	// 判断是否是空闲中断
            if (RESET != __HAL_UART_GET_FLAG(huart1, UART_FLAG_IDLE)) {
                // 计算接收到的数据长度
                uint32_t data_length = BUFFER_SIZE - __HAL_DMA_GET_COUNTER(&hdma_usart1_rx);
                __HAL_UART_CLEAR_IDLEFLAG(huart1);
                // 停止本次DMA传输
                HAL_UART_DMAStop(huart1);
                // 将数据丢给外部去处理
                while (HAL_UART_Receive_DMA(huart1, uart_rx_buff, BUFFER_SIZE) != HAL_OK) {
                    huart1->RxState = HAL_UART_STATE_READY;
                    __HAL_UNLOCK(huart1);
                }
    
                // 数据处理的回调
                common_uart_idle_callback(uart_rx_buff,data_length);
            }
        }
    }
    
    /**
     * 将数据使用DMA的方式发送出去
     * @param data
     * @param size
     */
    void common_uart_send(uint8_t* data ,uint16_t size){
        // 直到DMA空闲,才进行数据的发送
    /*    while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX_RX);
    
        while (HAL_UART_Transmit_DMA(&huart1, data, size) != HAL_OK);*/
        while (HAL_UART_GetState(&huart1) == HAL_UART_STATE_BUSY_TX_RX);
        auto *buf = new uint8_t[size];
        memcpy(buf, data, size);
        while (HAL_UART_Transmit_DMA(&huart1, data, size) != HAL_OK);
        delete[] buf;
    }
    
    
    /* 中断错误处理函数，在此处理overrun错误 */
    void HAL_UART_ErrorCallback1(UART_HandleTypeDef *huart)
    {
        if(__HAL_UART_GET_FLAG(huart,UART_FLAG_PE) != RESET)
        {
            __HAL_UART_CLEAR_OREFLAG(huart);
        }
    
        if(__HAL_UART_GET_FLAG(huart,UART_FLAG_FE) != RESET)
        {
            __HAL_UART_CLEAR_FEFLAG(huart);
        }
        if(__HAL_UART_GET_FLAG(huart,UART_FLAG_NE) != RESET)
        {
            __HAL_UART_CLEAR_NEFLAG(huart);
        }
        if(__HAL_UART_GET_FLAG(huart,UART_FLAG_ORE) != RESET)
        {
            __HAL_UART_CLEAR_OREFLAG(huart);
        }
    
        common_uart_init();
    }
    
    
    
    
    
    /*
    // 1. cubemax中配置一下DMA功能
    
    //2. 还需要在stm32f1xx_it.c 文件中的USART1_IRQHandler(void)这个方法中执行这个common_uart_idle_handle(&huart1);
    
    函数
    
    stm32f1xx_it.c中导入 #include "common_uart.h" 头文件
    
    //3.
     在cppmain RobotInit()初始化方法中调用common_uart_init();初始化
        
    
    
    
    
    
    
    //4. 在 cppmain中 调用以下方法 将速度发送出去 [ protocol.h  TXProtocol : 结构体封装好的 速度陀螺仪 数据 ]
        common_uart_send((uint8_t*)protocol, sizeof(TXProtocol));
    
    
    //5. 在 cppmain中 重写以下下方法 将接收发送过来的数据
    /**
     * 接收外部发送过来的数据
     * @param receive_buf
     * @param receive_len
     */
    void common_uart_idle_callback(uint8_t receive_buf[],uint16_t receive_len){
        // 蜂鸣器控制
        HAL_GPIO_TogglePin(BUZZER_PORT,BUZZER_PIN);
    }
    
    
    
    
    
    */

配置一下DMA功能

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 16][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 17][]

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 18][]

![20200913113828983.png][]

__HAL_UART_ENABLE_IT(&huart1, UART_IT_IDLE);

![20200913113852913.png][]

HAL_UART_Receive_DMA(&huart1, (uint8_t*)uart_rx_buff, BUFFER_SIZE);

还需要在stm32f1xx\_it.c 文件中的USART1\_IRQHandler(void)这个方法中执行这个common\_uart\_idle\_handle(&huart1);

函数

stm32f1xx_it.c中导入 #include "common_uart.h" 头文件

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 19][]

stm\_car已知左右轮的速度,我们推导当前小车前进的线速度和角速度

/*
     * 
        已知左右轮的速度,我们推导当前小车前进的线速度和角速度
     */
    float Car::getXVel(){
        float vel1 = wheel1->vel;
        float vel2 = wheel2->vel;
        float vel3 = wheel3->vel;
        float vel4 = wheel4->vel;
        //转换成线速度
        return (vel1+vel2+vel3+vel4)/4;
    }
    
    float Car::getYVel(){
        return 0;
    }
    /*
     */
    float Car::getAngularVel(){
        float vel1 = wheel1->vel;
        float vel2 = wheel2->vel;
        float vel3 = wheel3->vel;
        float vel4 = wheel4->vel;
        //转换成角速度
        float angular = (-vel1-vel3 + vel2 + vel4)/4/((WHEEL_AXIS+WHEEL_DISTANCE)*0.5);
        return angular;
    }

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 20][]

stm\_ros解析上位机串口发来的数据

/**
     * 接收外部 上位机 发送过来的数据
     * @param receive_buf
     * @param receive_len
     */
    void common_uart_idle_callback(uint8_t receive_buf[],uint16_t receive_len){
    
        //数据格式:  帧头0 帧头1 类型  长度  线速低位   线速度高位  角速度低位  角速度高位
        //数据格式: [0xce, 0xfa, 0x05, 4,   250,        0,         244,        1]
        uint8_t i = 0;
        while(i < receive_len){
            // 先找到帧头0
            if(receive_buf[i] == FLAG_HEAD0){
                // 再找帧头1
                if(receive_buf[i+1] == FLAG_HEAD1){
                    // 若进到这里来了,说明帧头完全匹配
                    // 获取类型
                    uint8_t type = receive_buf[i+2];
                    if(type == 0x05){  // 判断是否为上位机发送过来的速度信息
                        // 获取数据长度
                        uint8_t len = receive_buf[i+3];
                        // 判断是否有足够多的数据, 为了防止获取数据越界
                        if(i + 2 + len < receive_len ){
                            // 还原线速度
                            uint8_t vel_low = receive_buf[i+4];
                            uint8_t vel_high = receive_buf[i+5];
                            short vel = vel_high<<8|vel_low;
                            float velocity = (float)vel/1000;
                            // 还原角速度
                            uint8_t angular_low = receive_buf[i+6];
                            uint8_t angular_high = receive_buf[i+7];
                            short ang = angular_high<<8|angular_low;
                            float angular = (float)ang/1000;
                            // 为了表示数据在传输
                            HAL_GPIO_TogglePin(LED_PORT,LED_PIN);
                            // 为了表示数据在传输
    //                        HAL_GPIO_TogglePin(BUZZER_PORT,BUZZER_PIN);
                            car.updateVel(velocity,0,angular);
                        }
                    }
                }
            }
    
            i++;
        }
    
    }

stm\_16进制2字节转换成short    ===================2020=09-21

short ang = angular_high<<8|angular_low;

cpp\_ros计算惯性动量公式\_惯性矩阵

py\_ros计算惯性动量公式\_惯性矩阵

圆柱cylinder
    m:密度 r:半径 h:高度
    
                    ixx="${m*(3*r*r+h*h)/12}"
                    iyy="${m*(3*r*r+h*h)/12}"
                    izz="${m*r*r/2}" 
    
    夹爪 夹爪是两个长方体
    
                     ixx="${m*(h*h+d*d)/12}"
                    iyy="${m*(w*w+d*d)/12}" 
                    izz="${m*(w*w+h*h)/12}"

urdf\_marm.xacro简易机械臂

<?xml version="1.0"?>
    <robot name="marm" xmlns:xacro="http://www.ros.org/wiki/xacro">
    
        
        <material name="Black">
            <color rgba="0 0 0 1"/>
        </material>
        <material name="White">
            <color rgba="1 1 1 1"/>
        </material>
        <material name="Blue">
            <color rgba="0 0 1 1"/>
        </material>
        <material name="Red">
            <color rgba="1 0 0 1"/>
        </material>
    
        
        <xacro:property name="M_PI" value="3.14159"/>
    
        
        <xacro:property name="link0_radius" value="0.05" />
        <xacro:property name="link0_length" value="0.04" />
        <xacro:property name="link0_mass" value="1" />
    
        
        <xacro:property name="link1_radius" value="0.03" />
        <xacro:property name="link1_length" value="0.10" />
        <xacro:property name="link1_mass" value="1" />
    
        
        <xacro:property name="link2_radius" value="0.03" />
        <xacro:property name="link2_length" value="0.14" />
        <xacro:property name="link2_mass" value="0.8" />
    
        
        <xacro:property name="link3_radius" value="0.03" />
        <xacro:property name="link3_length" value="0.15" />
        <xacro:property name="link3_mass" value="0.8" />
    
        
        <xacro:property name="link4_radius" value="0.025" />
        <xacro:property name="link4_length" value="0.06" />
        <xacro:property name="link4_mass" value="0.7" />
    
        
        <xacro:property name="link5_radius" value="0.03" />
        <xacro:property name="link5_length" value="0.06" />
        <xacro:property name="link5_mass" value="0.7" />
    
        
        <xacro:property name="link6_radius" value="0.04" />
        <xacro:property name="link6_length" value="0.02" />
        <xacro:property name="link6_mass" value="0.6" />
    
        
        <xacro:property name="gripper_length" value="0.03" />
        <xacro:property name="gripper_width" value="0.01" />
        <xacro:property name="gripper_height" value="0.06" />
        <xacro:property name="gripper_mass" value="0.5" />
    
        
        <xacro:property name="grasp_frame_radius" value="0.001" />
    
        
        <xacro:macro name="cylinder_inertial_matrix" params="m r h">
            <inertial>
                <mass value="${m}" />
                <inertia ixx="${m*(3*r*r+h*h)/12}" ixy = "0" ixz = "0"
                    iyy="${m*(3*r*r+h*h)/12}" iyz = "0"
                    izz="${m*r*r/2}" /> 
            </inertial>
        </xacro:macro>
        
        <xacro:macro name="box_inertial_matrix" params="m w h d">
            <inertial>
                <mass value="${m}" />
                <inertia ixx="${m*(h*h+d*d)/12}" ixy = "0" ixz = "0"
                    iyy="${m*(w*w+d*d)/12}" iyz = "0"
                    izz="${m*(w*w+h*h)/12}" /> 
            </inertial>
        </xacro:macro>
    
        
        <link name="base_link">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <box size="0.001 0.001 0.001" />
                </geometry>
            </visual>
        </link>
    
        <joint name="base_joint" type="fixed">
            <origin xyz="0 0 ${link0_length/2}" rpy="0 0 0" />        
            <parent link="base_link"/>
            <child link="link0" />
        </joint>
    
        
        <link name="link0">
            <visual>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link0_radius}" length="${link0_length}"/>
                </geometry>
                <material name="White" />
            </visual>
            <collision>
                <origin xyz="0 0 0" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link0_radius}" length="${link0_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link0_mass}" r="${link0_radius}" h="${link0_length}"/>
        </link>
    
        <joint name="joint1" type="revolute">
            <parent link="link0"/>
            <child link="link1"/>
            <origin xyz="0 0 ${link0_length/2}" rpy="0 ${M_PI/2} 0" />
            <axis xyz="-1 0 0" />
            <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}"/>
        </joint>
    
        
        <link name="link1" >
            <visual>
                <origin xyz="-${link1_length/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link1_radius}" length="${link1_length}"/>
                </geometry>
                <material name="Blue" />
            </visual>
            <collision>
                <origin xyz="-${link1_length/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link1_radius}" length="${link1_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link1_mass}" r="${link1_radius}" h="${link1_length}"/>
        </link>
    
        <joint name="joint2" type="revolute">
            <parent link="link1"/>
            <child link="link2"/>
            <origin xyz="-${link1_length} 0 0.0" rpy="-${M_PI/2} 0 ${M_PI/2}" />
            <axis xyz="1 0 0" />
            <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}" />
        </joint>
    
        
        <link name="link2" >
            <visual>
                <origin xyz="0 0 ${link2_length/2}" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link2_radius}" length="${link2_length}"/>
                </geometry>
                <material name="White" />
            </visual>
    
            <collision>
                <origin xyz="0 0 ${link2_length/2}" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link2_radius}" length="${link2_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link2_mass}" r="${link2_radius}" h="${link2_length}"/>
        </link>
    
        <joint name="joint3" type="revolute">
            <parent link="link2"/>
            <child link="link3"/>
            <origin xyz="0 0 ${link2_length}" rpy="0 ${M_PI} 0" />
            <axis xyz="-1 0 0" />
            <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}" />
        </joint>
    
        
        <link name="link3" >
            <visual>
                <origin xyz="0 0 -${link3_length/2}" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link3_radius}" length="${link3_length}"/>
                </geometry>
                <material name="Blue" />
            </visual>
            <collision>
                <origin xyz="0 0 -${link3_length/2}" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link3_radius}" length="${link3_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link3_mass}" r="${link3_radius}" h="${link3_length}"/>
        </link>
    
        <joint name="joint4" type="revolute">
            <parent link="link3"/>
            <child link="link4"/>
            <origin xyz="0.0 0.0 -${link3_length}" rpy="0 ${M_PI/2} ${M_PI}" />
            <axis xyz="1 0 0" />
            <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}" />
        </joint>
    
        
        <link name="link4" >
            <visual>
                <origin xyz="${link4_length/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link4_radius}" length="${link4_length}"/>
                </geometry>
                <material name="Black" />
            </visual>
            <collision>
                <origin xyz="${link4_length/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link4_radius}" length="${link4_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link4_mass}" r="${link4_radius}" h="${link4_length}"/>
        </link>
    
        <joint name="joint5" type="revolute">
            <parent link="link4"/>
            <child link="link5"/>
            <origin xyz="${link4_length} 0.0 0.0" rpy="0 ${M_PI/2} 0" />
            <axis xyz="1 0 0" />
            <limit effort="300" velocity="1" lower="${-M_PI}" upper="${M_PI}" />
        </joint>
    
        
        <link name="link5">
            <visual>
                <origin xyz="0 0 ${link4_length/2}" rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link5_radius}" length="${link5_length}"/>
                </geometry>
                <material name="White" />
            </visual>
            <collision>
                <origin xyz="0 0 ${link4_length/2} " rpy="0 0 0" />
                <geometry>
                    <cylinder radius="${link5_radius}" length="${link5_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link5_mass}" r="${link5_radius}" h="${link5_length}"/>
        </link>
    
        <joint name="joint6" type="revolute">
            <parent link="link5"/>
            <child link="link6"/>
            <origin xyz="0 0 ${link4_length}" rpy="${1.5*M_PI} -${M_PI/2} 0" />
            <axis xyz="1 0 0" />
            <limit effort="300" velocity="1" lower="${-2*M_PI}" upper="${2*M_PI}" />
        </joint>
    
        
        <link name="link6">
            <visual>
                <origin xyz="${link6_length/2} 0 0 " rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link6_radius}" length="${link6_length}"/>
                </geometry>
                <material name="Blue" />
            </visual>
            <collision>
                <origin xyz="${link6_length/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <cylinder radius="${link6_radius}" length="${link6_length}"/>
                </geometry>
            </collision>
            <cylinder_inertial_matrix m="${link6_mass}" r="${link6_radius}" h="${link6_length}"/>
        </link>
    
        <joint name="finger_joint1" type="prismatic">
            <parent link="link6"/>
            <child link="gripper_finger_link1"/>
            <origin xyz="${link6_length} -0.03 0" rpy="0 0 0" />
            <axis xyz="0 1 0" />
            <limit effort="100" lower="0" upper="0.06" velocity="0.02"/>
        </joint>
    
        
        
        <link name="gripper_finger_link1">
            <visual>
                <origin xyz="${gripper_height/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <box size="${gripper_length} ${gripper_width} ${gripper_height}" />
                </geometry>
                <material name="White" />
            </visual>
            <collision>
                <origin xyz="${gripper_height/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <box size="${gripper_length} ${gripper_width} ${gripper_height}" />
                </geometry>
            </collision>
            <box_inertial_matrix m="${gripper_mass}" w="${gripper_width}" h="${gripper_height}" d="${gripper_length}"/>
        </link>
    
        <joint name="finger_joint2" type="fixed">
            <parent link="link6"/>
            <child link="gripper_finger_link2"/>
            <origin xyz="${link6_length} 0.03 0" rpy="0 0 0" />
        </joint>
    
        
        <link name="gripper_finger_link2">
            <visual>
                <origin xyz="${gripper_height/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <box size="${gripper_length} ${gripper_width} ${gripper_height}" />
                </geometry>
                <material name="White" />
            </visual>
            <collision>
                <origin xyz="${gripper_height/2} 0 0" rpy="0 ${M_PI/2} 0" />
                <geometry>
                    <box size="${gripper_length} ${gripper_width} ${gripper_height}" />
                </geometry>
            </collision>
            <box_inertial_matrix m="${gripper_mass}" w="${gripper_width}" h="${gripper_height}" d="${gripper_length}"/>
        </link>
    
        
        <link name="grasping_frame"/>
    
        <joint name="grasping_frame_joint" type="fixed">
            <parent link="link6"/>
            <child link="grasping_frame"/>
            <origin xyz="${gripper_height} 0 0" rpy="0 0 0"/>
        </joint>
    
    </robot>

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 21][]

launch\_view\_marm.launch显示urdf创建的机器人

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 22][]

launch\_joint\_state\_publishe可视化机器人操作

<param name="use_gui" value="true"/>
    
        
        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
        
        
        <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

cpp_ros_mat_类_Rotation3DUtils.h旋转矩阵_变换矩阵_齐次变换矩阵_欧拉角转旋转矩阵_等互转n工具类

//
    // Created by ty on 20-6-30.
    ///home/sk/workspas/HandEyeCalibration 无序拣选项目中
    //
    
    #ifndef CALIBRATIONHANDEYE_ROTATION3DUTILS_H
    #define CALIBRATIONHANDEYE_ROTATION3DUTILS_H
    
    #include <iostream>
    #include <opencv2/opencv.hpp>
    
    using namespace std;
    using namespace cv;
    
    /**
     * 检查是否是旋转矩阵
     *
     * 齐次正交矩阵
     * 矩阵的转置T 等于 矩阵的逆inv
    **/
    bool isRotationMatrix(Mat &R) {
        Mat Rt;
        transpose(R, Rt);
        // 应该得到单位矩阵
        Mat shouldBeIdentity = Rt * R;
        Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
    
        return norm(I, shouldBeIdentity) < 1e-6;
    }
    
    /**
     * 将旋转矩阵和平移向量转成4x4的齐次变换矩阵
     * @param rotationMat    旋转矩阵
     * @param translationMat 平移向量
     * @return 齐次变换矩阵
     */
    static Mat toHomogeneousMat(const Mat &rotationMat, const Mat &translationMat){
        Mat m = Mat::eye(4, 4, CV_64FC1); // rotationMat.type()
    
        Mat R = m(Rect(0, 0, 3, 3));
        rotationMat.convertTo(R, CV_64F);
    
        Mat t = m(Rect(3, 0, 1, 3));
        translationMat.convertTo(t, CV_64F);
        return m;
    }
    
    /**
     * 求齐次矩阵的逆矩阵
     * @param T
     * @return
     */
    static Mat homogeneousInverse(const Mat &T) {
        CV_Assert(T.rows == 4 && T.cols == 4);
    
        Mat R = T(Rect(0, 0, 3, 3));
        Mat t = T(Rect(3, 0, 1, 3));
    
        Mat Rt = R.t();
        Mat tinv = -Rt * t;
    
        Mat Tinv = Mat::eye(4, 4, T.type());
    
        Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
        tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
    
        return Tinv;
    }
    
    /**
     * 取出齐次变换矩阵的旋转矩阵和平移向量
     * @param T 4x4变换矩阵
     * @param outputRotation 旋转矩阵
     * @param outputTranslation 平移向量
     */
    static void splitHomogeneousMat(Mat &T, Mat& outputRotation, Mat& outputTranslation){
        CV_Assert(T.rows == 4 && T.cols == 4);
    
        Mat R = T(Rect(0, 0, 3, 3));
        Mat t = T(Rect(3, 0, 1, 3));
    
        R.copyTo(outputRotation);
        t.copyTo(outputTranslation);
    };
    
    /**
     * 欧拉角转旋转矩阵
     * @param theta
     * @return
     */
    Mat eulerAnglesToRotationMatrix(cv::Vec3f &theta){
    
        // X轴旋转矩阵
        Mat R_x = (Mat_<double>(3, 3) <<
                1, 0, 0,
                0, cos(theta[0]), -sin(theta[0]),
                0, sin(theta[0]), cos(theta[0])  );
        // Y轴旋转矩阵
        Mat R_y = (Mat_<double>(3, 3) <<
                cos(theta[1]), 0, sin(theta[1]),
                0, 1, 0,
                -sin(theta[1]), 0, cos(theta[1]));
        // Z轴旋转矩阵
        Mat R_z = (Mat_<double>(3, 3) <<
                cos(theta[2]), -sin(theta[2]), 0,
                sin(theta[2]), cos(theta[2]) , 0,
                0, 0, 1
        );
    
        Mat R = R_z * R_y * R_x;
        return R;
    }
    /**
     * 将旋转矩阵转成欧拉角
     * @param R 旋转矩阵
     * @return  欧拉角
     */
    Vec3f rotationMatrixToEulerAngles(cv::Mat &R){
        assert(isRotationMatrix(R));
    
        float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));
    
        bool singular = sy < 1e-6; // If
    
        float x, y, z;
        if (!singular) {
            x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
            y = atan2(-R.at<double>(2, 0), sy);
            z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
        } else {
            x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
            y = atan2(-R.at<double>(2, 0), sy);
            z = 0;
        }
        return Vec3f(x, y, z);
    };
    
    #endif //CALIBRATIONHANDEYE_ROTATION3DUTILS_H

cpp\_ros\_mat\_isRotationMatrix检查是否是旋转矩阵

/**
     * 检查是否是旋转矩阵
     *
     * 齐次正交矩阵
     * 矩阵的转置T 等于 矩阵的逆inv
    **/
    bool isRotationMatrix(Mat &R) {
        Mat Rt;
        transpose(R, Rt);
        // 应该得到单位矩阵
        Mat shouldBeIdentity = Rt * R;
        Mat I = Mat::eye(3, 3, shouldBeIdentity.type());
    
        return norm(I, shouldBeIdentity) < 1e-6;
    }

cpp\_ros\_mat\_toHomogeneousMat将旋转矩阵和平移向量转成4x4的齐次变换矩阵

/**
     * 将旋转矩阵和平移向量转成4x4的齐次变换矩阵
     * @param rotationMat    旋转矩阵
     * @param translationMat 平移向量
     * @return 齐次变换矩阵
     */
    static Mat toHomogeneousMat(const Mat &rotationMat, const Mat &translationMat){
        Mat m = Mat::eye(4, 4, CV_64FC1); // rotationMat.type()
    
        Mat R = m(Rect(0, 0, 3, 3));
        rotationMat.convertTo(R, CV_64F);
    
        Mat t = m(Rect(3, 0, 1, 3));
        translationMat.convertTo(t, CV_64F);
        return m;
    }

cpp\_ros\_mat\_homogeneousInverse求齐次矩阵的逆矩阵

/**
     * 求齐次矩阵的逆矩阵
     * @param T
     * @return
     */
    static Mat homogeneousInverse(const Mat &T) {
        CV_Assert(T.rows == 4 && T.cols == 4);
    
        Mat R = T(Rect(0, 0, 3, 3));
        Mat t = T(Rect(3, 0, 1, 3));
    
        Mat Rt = R.t();
        Mat tinv = -Rt * t;
    
        Mat Tinv = Mat::eye(4, 4, T.type());
    
        Rt.copyTo(Tinv(Rect(0, 0, 3, 3)));
        tinv.copyTo(Tinv(Rect(3, 0, 1, 3)));
    
        return Tinv;
    }

cpp\_ros\_mat\_splitHomogeneousMat取出齐次变换矩阵的旋转矩阵和平移向量

/**
     * 取出齐次变换矩阵的旋转矩阵和平移向量
     * @param T 4x4变换矩阵
     * @param outputRotation 旋转矩阵
     * @param outputTranslation 平移向量
     */
    static void splitHomogeneousMat(Mat &T, Mat& outputRotation, Mat& outputTranslation){
        CV_Assert(T.rows == 4 && T.cols == 4);
    
        Mat R = T(Rect(0, 0, 3, 3));
        Mat t = T(Rect(3, 0, 1, 3));
    
        R.copyTo(outputRotation);
        t.copyTo(outputTranslation);
    };

cpp\_ros\_mat\_eulerAnglesToRotationMatrix欧拉角转旋转矩阵

/**
     * 欧拉角转旋转矩阵
     * @param theta
     * @return
     */
    Mat eulerAnglesToRotationMatrix(cv::Vec3f &theta){
    
        // X轴旋转矩阵
        Mat R_x = (Mat_<double>(3, 3) <<
                1, 0, 0,
                0, cos(theta[0]), -sin(theta[0]),
                0, sin(theta[0]), cos(theta[0])  );
        // Y轴旋转矩阵
        Mat R_y = (Mat_<double>(3, 3) <<
                cos(theta[1]), 0, sin(theta[1]),
                0, 1, 0,
                -sin(theta[1]), 0, cos(theta[1]));
        // Z轴旋转矩阵
        Mat R_z = (Mat_<double>(3, 3) <<
                cos(theta[2]), -sin(theta[2]), 0,
                sin(theta[2]), cos(theta[2]) , 0,
                0, 0, 1
        );
    
        Mat R = R_z * R_y * R_x;
        return R;
    }
    
    
    //使用:
           // 旋转矩阵 RPY -> Rotation
            cv::Vec3f eulerAngles(pose[3], pose[4], pose[5]);
            const Mat &R = eulerAnglesToRotationMatrix(eulerAngles);

cpp\_ros\_mat\_rotationMatrixToEulerAngles将旋转矩阵转成欧拉角

/**
     * 将旋转矩阵转成欧拉角
     * @param R 旋转矩阵
     * @return  欧拉角
     */
    Vec3f rotationMatrixToEulerAngles(cv::Mat &R){
        assert(isRotationMatrix(R));
    
        float sy = sqrt(R.at<double>(0, 0) * R.at<double>(0, 0) + R.at<double>(1, 0) * R.at<double>(1, 0));
    
        bool singular = sy < 1e-6; // If
    
        float x, y, z;
        if (!singular) {
            x = atan2(R.at<double>(2, 1), R.at<double>(2, 2));
            y = atan2(-R.at<double>(2, 0), sy);
            z = atan2(R.at<double>(1, 0), R.at<double>(0, 0));
        } else {
            x = atan2(-R.at<double>(1, 2), R.at<double>(1, 1));
            y = atan2(-R.at<double>(2, 0), sy);
            z = 0;
        }
        return Vec3f(x, y, z);
    };
    
    //使用
        const Vec3f &euler = rotationMatrixToEulerAngles(R_camera2base);
    
        std::cout << "旋转矩阵：\n" << R_camera2base << std::endl;
        std::cout << "欧拉角：" << euler *  180.0f / M_PI << std::endl;

cpp_ros_xml_读取xml

#include <tinyxml2.h>
    
    #include <iostream>
    #include <vector>
    
    using namespace std;
    using namespace tinyxml2;
    
    typedef struct {
    
        std::string image_path; // 保存图片路径
    
        double joints[6]; // 保存关节角信息
    
        double pose[6];   // 保存位姿数据
    
    } Extrinsic;
    
    vector<Extrinsic> readXml(char* extrinsic_param_path) {// 读取xml
        XMLDocument doc;
        doc.LoadFile(extrinsic_param_path);
        XMLElement *root = doc.RootElement();
    
        XMLElement *child = root->FirstChildElement("extrinsic");
    
        vector<Extrinsic> exts;
    
    //    int counter = 0;
    
        while (child) {
            const char *image_path = child->FirstChildElement("Limage_color_path")->GetText();
    
            double joint1 = atof(child->FirstChildElement("joint1")->GetText());
            double joint2 = atof(child->FirstChildElement("joint2")->GetText());
            double joint3 = atof(child->FirstChildElement("joint3")->GetText());
            double joint4 = atof(child->FirstChildElement("joint4")->GetText());
            double joint5 = atof(child->FirstChildElement("joint5")->GetText());
            double joint6 = atof(child->FirstChildElement("joint6")->GetText());
    
            double pose1 = atof(child->FirstChildElement("pose1")->GetText());
            double pose2 = atof(child->FirstChildElement("pose2")->GetText());
            double pose3 = atof(child->FirstChildElement("pose3")->GetText());
            double pose4 = atof(child->FirstChildElement("pose4")->GetText());
            double pose5 = atof(child->FirstChildElement("pose5")->GetText());
            double pose6 = atof(child->FirstChildElement("pose6")->GetText());
    
    //        std::cout << "image_path: " << image_path << " joint1: " << joint1 << std::endl;
            exts.push_back((Extrinsic) {
                    std::string(image_path),
                    {joint1, joint2, joint3, joint4, joint5, joint6},
                    {pose1, pose2, pose3, pose4, pose5, pose6},
            });
    
            child = child->NextSiblingElement();
    
    //        counter++;
    //        if (counter == 5) {
    //            break;
    //        }
        }
    
        doc.Clear();
        return exts;
    }

cpp\_ros\_xml\_写出xml

#include <tinyxml2.h>
    
    #include <iostream>
    #include <vector>
    
    using namespace std;
    using namespace tinyxml2;
    
    typedef struct {
    
        std::string image_path; // 保存图片路径
    
        double joints[6]; // 保存关节角信息
    
        double pose[6];   // 保存位姿数据
    
    } Extrinsic;
    
    
    
    void writeXml(vector<Extrinsic> &exts, char *extrinsic_output_param_path) {// 写出xml
        XMLDocument *doc = new XMLDocument();
    
        XMLDeclaration *declaration = doc->NewDeclaration(R"(xml version="1.0" encoding="UTF-8" standalone="yes")");
        doc->InsertEndChild(declaration);
    
        XMLElement *root = doc->NewElement("ExtrinsicParams");
        doc->InsertEndChild(root);
    
        // 向root中添加数据
        unsigned long size = exts.size();
        for (int i = 0; i < size; ++i) {
            Extrinsic &ext = exts[i];
            // 创建子元素并添加给root
            XMLElement *child = doc->NewElement("extrinsic");
            child->SetAttribute("name", i + 1);
            root->InsertEndChild(child);
    
            // 图片路径
            XMLElement *imageElement = doc->NewElement("Limage_color_path");
            imageElement->SetText(ext.image_path.c_str());
            child->InsertEndChild(imageElement);
    
            // 关节角
            for (int j = 0; j < 6; ++j) {
                double joint = ext.joints[j];
                XMLElement *jointEle = doc->NewElement(("joint" + to_string(j + 1)).c_str());
                jointEle->SetText(joint);
                child->InsertEndChild(jointEle);
            }
    
            // 位姿
            for (int j = 0; j < 6; ++j) {
                double pose = ext.pose[j];
                XMLElement *poseEle = doc->NewElement(("pose" + to_string(j + 1)).c_str());
                poseEle->SetText(pose);
                child->InsertEndChild(poseEle);
            }
        }
    
        XMLError error = doc->SaveFile(extrinsic_output_param_path);
        if (error == XML_SUCCESS) {
            cout << "写出成功！" << endl;
        } else {
            cerr << "写出失败：" << error << endl;
        }
    
        doc->Clear();
    }

cpp_ros_xml_XmlUtil.h机械臂手眼标定位置_位姿等数据保存读取工具类

//
    // /home/sk/workspas/HandEyeCalibration/src/tinyxml/XmlUtil.h
    //
    
    #ifndef HANDEYECALIBRATION_XMLUTIL_H
    #define HANDEYECALIBRATION_XMLUTIL_H
    
    
    #include <tinyxml2.h>
    
    #include <iostream>
    #include <vector>
    
    using namespace std;
    using namespace tinyxml2;
    
    typedef struct {
    
        std::string image_path; // 保存图片路径
    
        double joints[6]; // 保存关节角信息
    
        double pose[6];   // 保存位姿数据
    
    } Extrinsic;
    
    vector<Extrinsic> readXml(char* extrinsic_param_path) {// 读取xml
        XMLDocument doc;
        doc.LoadFile(extrinsic_param_path);
        XMLElement *root = doc.RootElement();
    
        XMLElement *child = root->FirstChildElement("extrinsic");
    
        vector<Extrinsic> exts;
    
    //    int counter = 0;
    
        while (child) {
            const char *image_path = child->FirstChildElement("Limage_color_path")->GetText();
    
            double joint1 = atof(child->FirstChildElement("joint1")->GetText());
            double joint2 = atof(child->FirstChildElement("joint2")->GetText());
            double joint3 = atof(child->FirstChildElement("joint3")->GetText());
            double joint4 = atof(child->FirstChildElement("joint4")->GetText());
            double joint5 = atof(child->FirstChildElement("joint5")->GetText());
            double joint6 = atof(child->FirstChildElement("joint6")->GetText());
    
            double pose1 = atof(child->FirstChildElement("pose1")->GetText());
            double pose2 = atof(child->FirstChildElement("pose2")->GetText());
            double pose3 = atof(child->FirstChildElement("pose3")->GetText());
            double pose4 = atof(child->FirstChildElement("pose4")->GetText());
            double pose5 = atof(child->FirstChildElement("pose5")->GetText());
            double pose6 = atof(child->FirstChildElement("pose6")->GetText());
    
    //        std::cout << "image_path: " << image_path << " joint1: " << joint1 << std::endl;
            exts.push_back((Extrinsic) {
                    std::string(image_path),
                    {joint1, joint2, joint3, joint4, joint5, joint6},
                    {pose1, pose2, pose3, pose4, pose5, pose6},
            });
    
            child = child->NextSiblingElement();
    
    //        counter++;
    //        if (counter == 5) {
    //            break;
    //        }
        }
    
        doc.Clear();
        return exts;
    }
    
    void writeXml(vector<Extrinsic> &exts, char *extrinsic_output_param_path) {// 写出xml
        XMLDocument *doc = new XMLDocument();
    
        XMLDeclaration *declaration = doc->NewDeclaration(R"(xml version="1.0" encoding="UTF-8" standalone="yes")");
        doc->InsertEndChild(declaration);
    
        XMLElement *root = doc->NewElement("ExtrinsicParams");
        doc->InsertEndChild(root);
    
        // 向root中添加数据
        unsigned long size = exts.size();
        for (int i = 0; i < size; ++i) {
            Extrinsic &ext = exts[i];
            // 创建子元素并添加给root
            XMLElement *child = doc->NewElement("extrinsic");
            child->SetAttribute("name", i + 1);
            root->InsertEndChild(child);
    
            // 图片路径
            XMLElement *imageElement = doc->NewElement("Limage_color_path");
            imageElement->SetText(ext.image_path.c_str());
            child->InsertEndChild(imageElement);
    
            // 关节角
            for (int j = 0; j < 6; ++j) {
                double joint = ext.joints[j];
                XMLElement *jointEle = doc->NewElement(("joint" + to_string(j + 1)).c_str());
                jointEle->SetText(joint);
                child->InsertEndChild(jointEle);
            }
    
            // 位姿
            for (int j = 0; j < 6; ++j) {
                double pose = ext.pose[j];
                XMLElement *poseEle = doc->NewElement(("pose" + to_string(j + 1)).c_str());
                poseEle->SetText(pose);
                child->InsertEndChild(poseEle);
            }
        }
    
        XMLError error = doc->SaveFile(extrinsic_output_param_path);
        if (error == XML_SUCCESS) {
            cout << "写出成功！" << endl;
        } else {
            cerr << "写出失败：" << error << endl;
        }
    
        doc->Clear();
    }
    
    #endif //HANDEYECALIBRATION_XMLUTIL_H

cpp\_opencv\_char字节数组转mat图像矩阵

//libfreenect2::Frame *rgb = frames[libfreenect2::Frame::Color];
        //unsigned char* data;  char*  rgb->data 图片的字节s数组
        Mat colorMat = Mat(rgb->height, rgb->width, CV_8UC4, rgb->data);

cpp\_opencv\_flip图像进行镜像处理

// 图像进行镜像处理
        flip(colorMat, colorMat, 1);

cpp\_opencv\_copyTo图像拷贝

Mat outputMat;
    colorMat.copyTo(outputMat);

cpp\_opencv\_resize设置图片大小

resize(colorMat, colorMat, Size(), 0.5, 0.5);

cpp\_opencv\_eye创建3\*3的对角矩阵

Mat cameraMatrix = Mat::eye(3, 3, CV_64F); // out

cpp\_opencv\_zeros创建5\*1的矩阵

Mat distCoeffs = Mat::zeros(5, 1, CV_64F); // out

cpp_opencv_Size定义宽高

Size patternSize(6, 9);

cpp\_opencv\_glob遍历文件夹，读取所有图片，转成Mat

// 遍历文件夹，读取所有图片，转成Mat
        vector<String> filesPaths;
        cv::glob(image_save_path + "/image_*.jpg", filesPaths);
    
        for (String image_path: filesPaths) {
            std::cout << "image_path: " << image_path << std::endl;
            Mat colorMat = imread(image_path, IMREAD_COLOR);
    
    }

cpp\_opencv\_平移矩阵\_xyz坐标转平移矩阵

// 平移矩阵
            Mat t = (Mat_<double>(3, 1) << pose[0], pose[1], pose[2]);
            std::cout << "t: " << t << std::endl;

cpp\_opencv坐标转平移矩阵\_欧拉角转旋转矩阵

// 旋转矩阵 RPY -> Rotation
            cv::Vec3f eulerAngles(pose[3], pose[4], pose[5]);
            const Mat &R = eulerAnglesToRotationMatrix(eulerAngles);
            std::cout << "eulerAngles: " << eulerAngles << std::endl;
            std::cout << "R: \n" << R << std::endl;
    
            // 平移矩阵
            Mat t = (Mat_<double>(3, 1) << pose[0], pose[1], pose[2]);
            std::cout << "t: " << t << std::endl;

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70]: /images/20221123/f42e6e56f23e44c98a8682a8c72d6bfe.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 1]: /images/20221123/b783f12d3ac245adb4a8afc94ecbe9b9.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 2]: /images/20221123/bc145a410bc24b93a83f1b3457f92477.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 3]: /images/20221123/a29cb94a286a41ffbd3997856f77aa16.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 4]: /images/20221123/24e46c84256c4abf954a65bfc1e49d4c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 5]: /images/20221123/18f27a4cf06d44fc81547dfd5232c083.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 6]: /images/20221123/e9d2a05286644881a28fc61b58f0e288.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 7]: /images/20221123/88b2e85ad18a4f38b334f8eeb443ef26.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 8]: /images/20221123/79bb8952842048b19135f34e81825ace.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 9]: /images/20221123/9f72c858f7254c7a82ec678b8f652853.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 10]: /images/20221123/60bace5102d6470ab8e955091d81a9e7.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 11]: /images/20221123/d64fe9e6270744dcbb553213acdb25eb.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 12]: /images/20221123/7910eb1cf2664200a2989e1c84f2ea11.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 13]: /images/20221123/f5862eaa817846d988ad9bd894eada15.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 14]: /images/20221123/b918d7c0a1cd48e7890d752872c74171.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 15]: /images/20221123/7f49ae8c3ba144dc99ba8d72a6e8d0e5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 16]: /images/20221123/731cb7b2e9434dafbd2811fcf96f9213.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 17]: /images/20221123/6040659eac0e494096f890d5494bfea4.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 18]: /images/20221123/0c7133341f1845b3a0b7b228944277c4.png
[20200913113828983.png]: /images/20221123/ecd98e8e29704090b25d056e56b00b1c.png
[20200913113852913.png]: /images/20221123/4f22eec7691b4ac5a05caf9ef4ae55ef.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 19]: /images/20221123/2f1d1e1fac0f4cc898dfbc78004faaff.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 20]: /images/20221123/40ee4be643a3467dab1e60cc02ca351c.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 21]: /images/20221123/b2f3d27615cf4ef7943a92512bc5f721.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzE1MjA0MTc5_size_16_color_FFFFFF_t_70 22]: /images/20221123/3e16a22f930b4210963c42d77a860161.png