平衡小车实现

2023-05-16

平衡小车

1. 前期准备

1.1 I2C通讯协议

在与MPU6050进行数据的读取时需要用到I2C通讯协议来进行通信。

物理层

IIC一共有只有两个总线：一条是双向的串行数据线SDA，一条是串行时钟线SCL

SDA(Serial data)是数据线，D代表Data也就是数据，Send Data 也就是用来传输数据的

SCL(Serial clock line)是时钟线，C代表Clock 也就是时钟也就是控制数据发送的时序的

所有接到I2C总线设备上的串行数据SDA都接到总线的SDA上，各设备的时钟线SCL接到总线的SCL上。I2C总线上的每个设备都自己一个唯一的地址，来确保不同设备之间访问的准确性.

I2C 总线在物理连接上非常简单，分别由SDA(串行数据线)和SCL(串行时钟线)及上拉电阻组成。通信原理是通过对SCL和SDA线高低电平时序的控制，来产生I2C总线协议所需要的信号进行数据的传递。在总线空闲状态时，SCL和SDA被上拉电阻Rp拉高，使SDA和SCL线都保持高电平。

I2C通信方式为半双工，只有一根SDA线，同一时间只可以单向通信，485也为半双工，SPI和uart通信为全双工。

协议层

协议时序图如下

I2C 总线在传送数据过程中共有三种类型信号，它们分别是：开始信号、结束信号和应答信号。

开始信号：SCL 为高电平时，SDA 由高电平向低电平跳变，开始传送数据。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-CDUvvtKX-1671977201530)(https://www.houenup.com/wp-content/uploads/2022/12/图片2.png)]

结束信号：SCL 为高电平时，SDA 由低电平向高电平跳变，结束传送数据。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-NzF0iBRL-1671977201530)(https://www.houenup.com/wp-content/uploads/2022/12/图片3.png)]

应答信号：主机SCL拉高，读取从机SDA的电平，为低电平表示产生应答。

每当主机向从机发送完一个字节的数据，主机总是需要等待从机给出一个应答信号，以确认从机是否成功接收到了数据。数据传输时许如下

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yewYN8aS-1671977201530)(https://www.houenup.com/wp-content/uploads/2022/12/图片4.png)]

1.2 PID控制理论

PID算是很基础并且实用性很高的控制算法了，它的公式如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-A3Z7ujf0-1671977201531)(https://www.houenup.com/wp-content/uploads/2022/12/图片5.png)]

比例系数 kp 的作用是加快系统的响应速度，提高系统的调节精度。 kp 越大，系统

的响应速度越快，系统的调节精度越高，但易产生超调，甚至会导致系统不稳定。kp 取值过小，则会降低调节精度，使响应速度缓慢，从而延长调节时间，使系统静态、动态特性变坏。

积分作用系数ki 的作用是消除系统的稳态误差。ki 越大，系统的静态误差消除越快，

但 ki 过大，在响应过程的初期会产生积分饱和现象，从而引起响应过程的较大超调。若 ki 过小，将使系统静态误差难以消除，影响系统的调节精度。

微分作用系数 kd 的作用是改善系统的动态特性，其作用主要是在响应过程中抑制偏差向任何方向的变化，对偏差变化进行提前预报。但kd 过大，会使响应过程提前制动，从而延长调节时间，而且会降低系统的抗干扰性能。

PID 参数的整定必须考虑到在不同时刻三个参数的作用及相互之间的关系。

1.3 系统建模

在平衡小车中可以分为三个部分：检测部分、控制部分、执行部分。在控制的部分可以分为两个方面：对角度的控制、对速度的控制。检测部分采用角度传感器来感知车的姿态变化、控制部分采用MCU搭配PID控制算法，最后控制部分输出控制量传给执行部分进行执行。

因此初步建模如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-RBkFRNSG-1671977201531)(https://www.houenup.com/wp-content/uploads/2022/12/图片6.png)]

1.4 部件选型

MCU：STM32F103C8T6最小系统板

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-yeX4pgoW-1671977201531)(https://www.houenup.com/wp-content/uploads/2022/12/图片7.png)]

选择原因：接口丰富、运算速度足以满足PID控制算法的需求、定时器拥有输入捕获功能，方便对电机的控制和对电机状态的分析。

IMU：MPU6050

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-G9k1pvxz-1671977201531)(https://www.houenup.com/wp-content/uploads/2022/12/图片8.png)]

选择原因：成本低、功能强大、精度可满足需求。

电机：带霍尔编码器电机

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-OGFwlf4m-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片9.png)]

选择原因：简单的TT直流减速电机无法精准地对其运动进行控制，并且产品质量低，规格不统一（例如：两个相同的电机扭矩差距很大）难以控制。因此选择可以通过分析或电机速度的带编码器的电机。

电机驱动板：集成化电机驱动板
[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-H6FyS7SU-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片11.jpg)]

选择原因：体型小巧，相比于L298N更加节省空间。采用与电机配套的接口，减少杜邦线的使用，更加可靠。自带开关，方便对输出机构进行控制。接口丰富。

2 具体流程实现

在具体实现的时候可以将这个工程划分为两大部分：外设功能实现部分、任务逻辑控制部分。

同时通过在STM32上移植了FreeRTOS实时操作系统，实现多任务“同时”运行的效果，并且通过任务的划分，让整个处理流程变得更加有序、清晰。

整个代码结构如下:

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-KoTrxDkP-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片12.png)]

依次为：算法部分、任务逻辑部分、以及BSP部分。

用FreeRTOS开辟了三个任务进程：

计算进程：主要负责对电机的信息的解算以及PID的运算，将输出结果传给控制进程。

控制进程：负责输出PWM波来实现对电机的控制。

调试进程：负责处理整个系统的一些异常（例如，车的倾角过大时对电机的关闭与重启）

在BSP中放了软件I2C的控制、MPU6050的通信与姿态解算程序。这一部分是整车功能实现的基石。下面先从这部分开始依次进行讲解。

2.1 MPU6050数据读取

MPU6050数据读取需要I2C通讯的支持，因此需要先实现I2C的功能。

2.1.1 I2C通讯

STM32进行I2C通讯的时候有两个选择：一是使用其本身自带的硬件I2C、二是使用I0软件模拟I2C称作软件I2C。

硬件I2C：

优点：操作方便，可以直接调用STM32给出的外设API接口直接操控I2C外设。并且速度快、效率高。

缺点：IO口有限制，因为硬件I2C外设与IO口是绑定在一起的，因此只能使用固定的几个引脚来实现通讯，不够自由。并且如果出现问题不好调试。

软件I2C：

优点：可以自定义引脚来当作I2C的SCL和SDA，自由灵活、方便接线与移植。

缺点：效率相较于硬件I2C较低。

最终选择软件I2C来进行通讯。

软件I2C要实现的功能有：发送开始信号、发送结束信号、发送应答/非应答信号、发送字节数据、读取字节数据。

2.1.1.1 注意事项

引脚方面

首先是端口的相关配置需要注意的地方。在I2C通讯的时候引脚的输出需要使用开漏上拉输出，因为I2C通讯有时候需要在I2C总线上挂载多个设备，如果不用开漏输出, 而用推挽输出, 会出现主设备之间短路的情况。如图所示。多个GPIO口连接在同一根线上, 某个GPIO输出高电平, 另一个GPIO输出低电平.,如果使用推挽输出, 这个GPIO的VCC和另一个GPIO的GND接在了一起, 也就是短路了,会导致设备的损坏

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QozBHwN6-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片13.jpg)]

并且在I2C挂载多个设备的时候需要通过线与的特性进行优先级的仲裁，而开漏输出具有线与的特性。有一个设备拉低总线，那么这个总线就为低，除非全部设备释放总线，这个总线才空闲。

需要用上拉输出的原因是开漏输出没有输出高电平的能力。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-JtaLZz2t-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片14.png)]

开漏输出时P-MOS永远截止，输出低电平时N-MOS导通接地输出低电平。输出高电平时N-MOS也截止，此时引脚为高阻态，相当于引脚连着无穷大的电阻。想要输出高电平则必须配备上拉电阻。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-FC2bCT95-1671977201532)(https://www.houenup.com/wp-content/uploads/2022/12/图片15.png)]

Stm32并不像51单片机那样引脚是默认的双向端口即可以读引脚电平又可以输出电平。STM32中的推挽输出是无法读出当前引脚上的电平的，因此要是用推挽输出的话在需要读取引脚电平的时候转换引脚模式为输入模式。而开漏输出则即可读又可写，类似51。

时序方面

I2C通信中对于时间是有要求的，需要在固定的状态保持一定的时间才会被认定为发送了对应的信号。同时I2C通讯对于电平的翻转也是有耐受程度的，如果翻转过快，通讯会出问题。

我们可以通过MPU6050的手册来看一下我们是否需要在一些地方进行延时。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-MwoIWcim-1671977201533)(https://www.houenup.com/wp-content/uploads/2022/12/图片16.png)]

可以看到SCL的最大频率不能拆过1MHZ，而stm32IO最快翻转频率可以达到50MHZ，因此我们在进行I2C通讯的时候最好选择低速（2MHZ）IO，并在快速上下拉的时候进行延时。

2.1.1.2 具体代码实现

/*! 发送开始信号
 * 该信号的判定条件：SCL高电平期间，SDA从高电平切换到低电平
 * 该信号前SCL与SDA的状态：  1.初始的时候出现：SCL、SDA空闲，被上拉，两者都为高电平(1)
 *                       2.在读取应答信号之后重新开始一帧数据时出现：此时SDA可能为高电平也可能为低电平 SCL为低电平(0)
 * 该信号后SCL与SDA的状态： SDA为0  SCL为0   总线忙
 * */

void I2C_Start()
{
    SDA_UP();               //保证SDA为高电平
    SCL_UP();
    DELAY();
    SDA_DOWN();
    DELAY();        //至少保持0.6us
    SCL_DOWN();             //钳住I2C总线，准备发送或接收
}

/*!  发送结束信号
 * 该信号的判定条件：SCL高电平期间，SDA从低电平切换到高电平
 * 该信号前SCL与SDA的状态：  1. 跟在产生应答信号之后    SDA可能是0 可能是1   我们掌握总线控制权 所以SCL为0
 *                       2. 跟在读取应答信号之后    SDA可能是0 可能是1    我们掌握总线控制权 所以SCL为0
 * 该信号后SCL与SDA的状态： SDA为1  SCL为1  符合空闲状态
 * */
void I2C_Stop()
{
    SDA_DOWN();         //保证SDA低电平从而可以产生上升沿
    SCL_UP();
    DELAY();
    SDA_UP();
    DELAY();
}


/*! 发送应答信号
 * 该信号的判定条件：SDA低电平期间 SCL拉高再拉低
 * 该信号前SCL与SDA的状态：跟在发送数据之后    SDA可能为0 可能为1， 我们掌握总线控制权 所以SCL为0
 * */
void I2C_Send_ACK()
{
    SDA_DOWN();
    SCL_UP();
    DELAY();
    SCL_DOWN();
与IMU通讯
与MPU6050的通讯就是在I2C的基础上来向MPU6050的对应寄存器中写对应的数据，然后读对应的寄存器来得到我们想要的数据。
具体实现代码

/*! @brief 写一个字节数据
 * @param reg：写入的寄存器地址
 * @param data：写入的数据
 * @return 1：写入失败 0：写入成功
 * */
uint8_t MPU_Write_Byte(uint8_t reg,uint8_t data)
{
    I2C_Start();
    I2C_Send_Byte(MPU_WRITE);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1;
    }
    I2C_Send_Byte(reg);
    I2C_Wait_ACK();
    I2C_Send_Byte(data);
    if (I2C_Wait_ACK())
    {
        I2C_Stop();
        return 1;
    }
    I2C_Stop();
    return 0;
}


/*!@brief  读一个字节 数据
 * @param reg：要读的寄存器地址
 * @return 读出的数据
 * */
uint8_t MPU_Read_Byte(uint8_t reg)
{
    uint8_t res;
    I2C_Start();
    I2C_Send_Byte((MPU6050_ADDRESS<<1)|0);       //发送器件地址+写命令
    I2C_Wait_ACK();                                     //等待应答
    I2C_Send_Byte(reg);                          //写寄存器地址
    I2C_Wait_ACK();                                     //等待应答
    I2C_Start();
    I2C_Send_Byte((MPU6050_ADDRESS<<1)|1);      //发送器件地址+读命令
    I2C_Wait_ACK();                                    //等待应答
    res=I2C_Receive_Byte(0);                          //读取数据,发送nACK
    I2C_Stop();                                       //产生一个停止条件
    return res;
}

通讯效果

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aT2tMan7-1671977201533)(https://www.houenup.com/wp-content/uploads/2022/12/图片17.jpg)]

2.1.2 MPU6050解算

从mpu6050中读出来的数据是原始数据，我们需要对这些数据进行解算才能直接用于我们的控制当中。这个结算的过程是相对复杂的，但是MPU6050官方给了一个dmp库，这个库中对MPU6050进行了姿态解算，并且进行了滤波。但是这个库写给MSP430等单片机用的，因此我需要进行对应的移植才可以使用。

代码实现部分

#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寄存器

//如果AD0脚(9脚)接地,IIC地址为0X68(不包含最低位).
//如果接V3.3,则IIC地址为0X69(不包含最低位).
#define MPU_ADDR            0X68

/*!
 * @brief: MPU初始化
 * */
uint8_t MPU_Init()
{
    uint8_t res;
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);    //复位MPU6050
    MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);    //唤醒MPU6050
    MPU_Set_Gyro_Fsr(3);               //陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);              //加速度传感器,±2g
    MPU_Set_Rate(50);                 //设置采样率50Hz
    MPU_Write_Byte(MPU_INT_EN_REG,0X00);   //关闭所有中断
    MPU_Write_Byte(MPU_USER_CTRL_REG,0X00);    //I2C主模式关闭
    MPU_Write_Byte(MPU_FIFO_EN_REG,0X00);  //关闭FIFO
    MPU_Write_Byte(MPU_INTBP_CFG_REG,0X80);    //INT引脚低电平有效
    res=MPU_Read_Byte(MPU_DEVICE_ID_REG);
    if(res==MPU_ADDR)//器件ID正确
    {
        MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);    //设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);    //加速度与陀螺仪都工作
        MPU_Set_Rate(50);                 //设置采样率为50Hz
    }else return 1;
    return 0;
}

例如这里，通过向MPU6050的0x6B寄存器中写入0x80便可以复位MPU6050。而向哪些寄存器中写入哪些数据可以实现什么样的功能可以通过查阅MPU6050的数据手册得知。

2.2 电机数据获取

现在很多高端电机都可以通过控制与其适配的电调来进行控制电机并且获取电机返回的例如角度、速度等数据。但是也有很多的低端电机没有电调，那么这是想要获取它的速度的时候就需要我们自己把电调的事儿给干了。

编码器电机给我们的反馈是编码器输出的脉冲，我们可以利用stm32定时器的编码器模式来获取编码器输出的脉冲数，从而得到电机的转动角度、转动速度等信息。

所使用的电机转动一圈输出500个脉冲，stm32定时器的编码器模式可以设置500个清零，也就是电机会不停地输出0~500的数来表示自己的转动的角度。可以通过这个来知道转了多少圈。并且我们可以通过单位时间内转过的角度来得到电机的速度。

但是计算速度的时候存在过零点的问题需要注意一下。

定义以下结构体：

#define FILTER_BUF        4       //累计几次产生一次速度
#define PULSE_ONE_LAP     500.0     //一圈产生500个脉冲
#define DELAT_T           5.0       //计算周期是5ms
#define SPEED_MAX         360     //rpm
typedef struct
{
    uint16_t ecd;                   //电机的编码器数值
    uint16_t last_ecd;              //上一次电机的编码器数值
    float  speed_rpm;               // rpm/60000*50  即毫秒脉冲数*10
    int32_t  round_cnt;             //电机旋转的总圈数
    int32_t  total_ecd;             //电机旋转的总编码器数值
    int32_t  total_angle;           //电机旋转的总角度
    int32_t total_ecd_last;
    int32_t total_ecd_delta;
    int32_t ecd_raw_rate;           //速度计算中间值
    int32_t  rate_buf[FILTER_BUF];  //多次取值取平均来计算速度
    uint8_t  buf_cut;
    uint32_t filter_rate;           //多次计算去平均过滤后的速度
    int32_t temp_sum;
}motor_measure_t;


void encoder_data_handle(motor_measure_t *ptr ,uint16_t data)
{
    ptr->temp_sum = 0;
    ptr->last_ecd      = ptr->ecd;
    ptr->ecd = data;
    if (ptr->ecd - ptr->last_ecd > (PULSE_ONE_LAP/2))
    {
        ptr->round_cnt--;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd - PULSE_ONE_LAP;
    }
    else if (ptr->ecd - ptr->last_ecd < -(PULSE_ONE_LAP/2))
    {
        ptr->round_cnt++;
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd + PULSE_ONE_LAP;
    }
    else
    {
        ptr->ecd_raw_rate = ptr->ecd - ptr->last_ecd;
    }
    ptr->total_ecd_last = ptr->total_ecd;
    ptr->total_ecd = ptr->round_cnt * PULSE_ONE_LAP + ptr->ecd;
    ptr->total_ecd_delta = ptr->total_ecd-ptr->total_ecd_last;
    ptr->total_angle = ptr->total_ecd * 360 / PULSE_ONE_LAP;
    ptr->speed_rpm = (((ptr->ecd_raw_rate/DELAT_T)))*10.0;
    if (ptr->speed_rpm>SPEED_MAX)
    {
        ptr->speed_rpm = SPEED_MAX;
    }
    if(ptr->speed_rpm<-SPEED_MAX)
    {
        ptr->speed_rpm = -SPEED_MAX;
    }

//            (float)(((float)ptr->ecd_raw_rate/(float)(PULSE_ONE_LAP*DELAT_T))*1000*60);
}

2.3 运算进程

2.3.1 任务周期

在Calculate任务中实现对轮子速度的获取以及PID的运算。因为PID的积分项与微分项对于计算周期比较敏感因此需要将该任务的优先级提高、设置任务执行周期为固定周期。

FreeRTOS可以很方便地完成这一操作。

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-fqw7QPIj-1671977201533)(https://www.houenup.com/wp-content/uploads/2022/12/图片18.png)]

将该任务的执行周期设置为5ms。

通过逻辑分析仪验证如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IgQRAtwT-1671977201533)(https://www.houenup.com/wp-content/uploads/2022/12/图片19.jpg)]

可以看到很精准。

2.3.2 PID运算与参数整定

在这个系统中我用的是串级PID，也就是角度环的输出作为速度环的输入。为什么需要速度环：因为角度环输出期望速度以后就不管了，如果直接将这个速度让电机来执行，空载的话还好，如果是有负载尤其是负载还变化的话那么电机输出的速度就不是期望的速度，因此再加一个速度环可以保证电机实际输出的就是角度需要的。

2.3.2.1 PID运算

PID控制器定义：

/**
  * @brief     PID控制器 结构体
  */
typedef struct
{
    /* p、i、d参数 */
    float p;
    float i;
    float d;

    /* 目标值、反馈值、误差值 */
    float set;
    float get;
    float err[2];

    /* p、i、d各项计算出的输出 */
    float pout;
    float iout;
    float dout;

    /*上一次的d输出值*/
    float last_dout;

    /* pid公式计算出的总输出 */
    float out;

    /* pid最大输出限制  */
    uint32_t max_output;

    /* pid积分输出项限幅 */
    uint32_t integral_limit;

    /* 输出死区 */
    uint32_t death_space;
    /*滤波比率*/
    float filtering_rate;

} pid_t;

PID计算：

/**
  * @brief     PID 计算函数，使用位置式 PID 计算
  * @param[in] pid: PID 结构体
  * @param[in] get: 反馈数据
  * @param[in] set: 目标数据
  * @retval    PID 计算输出
  */
float pid_calc(pid_t *pid, float get, float set)
{
    pid->get = get;
    pid->set = set;
    pid->err[NOW] = set - get;

    pid->pout = pid->p * pid->err[NOW];
    pid->iout += pid->i * pid->err[NOW];
    pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST]);

    abs_limit(&(pid->iout), pid->integral_limit);
    if (set==0)
    {
        pid->out = pid->pout + pid->iout + pid->dout;
    }
    else{
        pid->out = pid->pout + pid->iout + pid->dout + pid->death_space;
    }

    abs_limit(&(pid->out), pid->max_output);

    pid->err[LAST]  = pid->err[NOW];

    return pid->out;
}

2.3.2.2 PID整定

先从内环速度环开始整定，整定完成以后就可以认为给电机什么样的期望它就一定会输出什么样的速度，内环就可以盖住不管了，然后再调整外环角度环即可。

整定结果：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-jvzA2NW4-1671977201533)(https://www.houenup.com/wp-content/uploads/2022/12/图片20.jpg)]

2.4 控制进程与Debug进程

控制进程就是单纯地执行给电机PWM脉冲的任务。单独地将这一步分离出来可以让整个流程模块化，好调试。

在Debug进程中实现了小车的倾倒停机与重启功能。具体的做法就是在角度大于某个角度的时候便关闭电机。并且还可以在小车IMU初始化效果不好的时候，人工手摇小车实现IMU的重置

控制进程代码实现

void ControlTask(void const * argument)
{
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
    HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_4);
    uint32_t Contiol_time = osKernelSysTick();
    for(;;)
    {
//        Motor_Left_Move(PID_Speed_Lout);
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        Motor_Left_Move(PID_Speed_Lout);
//        osDelay(1);
        Motor_Right_Move(PID_Speed_Rout);
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_RESET);
//        osDelay(1);
        osDelayUntil(&Contiol_time,2);
    }

}


void Motor_Left_Move(int duty)
{
    if (duty>=0)
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, duty);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, 0);
    } else
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, 0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, -duty);
    }

}

void Motor_Right_Move(int duty)
{
    if (duty>=0)
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, duty);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);
    } else
    {
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3,0);
        __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, -duty);
    }
}

Debug进程代码实现

void DebugTask(void const * argument)
{
    HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
    uint32_t debug_time = osKernelSysTick();  //20ms为一个周期
    for(;;)
    {
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        if (MpuResetFlag)
        {
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
            PID_Speed_Lout = 0;
            PID_Speed_Rout = 0;
            while (MPU_Init())
            {
                ;
            }
            while (mpu_dmp_init())
            {
                ;
            }
            for (int i = 0; i < 20; i++) {
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
                HAL_Delay(500);
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
                HAL_Delay(500);
            }
            pitch_sum = 0;
            for (int i = 0; i < 30; i++) {
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
                for (int j = 0; j < 20; j++) {
                    mpu_dmp_get_data(&pitch,&roll,&yaw);
                    vTaskDelay(1);
                    pitch_sum+=pitch;
                    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
                }

            }
            offset = pitch_sum/600.0;
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
            MpuResetCompleteFlag = 1;
            MpuResetFlag = 0;
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
        }
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_RESET);
        osDelayUntil(&debug_time,20);
    }
}

3. 遇到的问题

3.1 电机速度不平滑

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-voTkYZNd-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片21.jpg)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-IHHtIZhT-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片22.jpg)]

正常的PID收敛效果应该是这样的：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-Bw01dL4t-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片23.jpg)]

原因：检测到的速度都是整数，而不是浮点数。

因为在计算转动速度的时候向把编码器的变化率转换成rpm（转每分）用到的公式是：

 (float)(((float)ptr->ecd_raw_rate/(float)(PULSE_ONE_LAP*DELAT_T))*1000*60);

发现因为里面要乘60000，如果想要保持rpm这个单位，就会是整数。处理的方法是，单位是什么并没必要，找到合适的映射值即可。

3.2 MPU6050数据不稳定

通过观察IMU读取出来的数据，发现虽然MPU6050初始化的时候只有保证平稳才能初始化成功，但是初始化成功之后的一段时间里IMU读取的数据偏离真实值，一段时间之后才能慢慢稳定在一个数的范围。但是稳定的地方并不一定是我们期望的0处，有可能存在一些偏差

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-0r2noP4I-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片24.jpg)]

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-n3idLGzX-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片25.jpg)]

解决方法：

面对要过一阵才能收敛的问题，通过观察发现，都是在初始化完成20s以后达到一个稳定的数值，因为不清楚这个问题出现的原因，目前能做的只是让程序等待20s达到稳定以后再作为角度环的输入值。

代码实现：

//等待imu数据稳定
for (int i = 0; i < 20; i++) {
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
    HAL_Delay(500);
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
    HAL_Delay(500);
}

至于稳定以后偏离机械中值的问题，可以通过取稳定以后的数据当作偏离值offset，在当作pid输入值的时候减去这个offset。需要注意的是不可以直接修改imu读取的pitch轴使其等于pitch减去偏离值，因为IMU在读取数据的时候是需要用到这个pitch数据的。只能拷贝一次将拷贝值减去偏离值当作pid的输入传进去。

只取一次值过于随机，因此要多取几次值取平均来作为偏移量。

代码实现：

//多次读取取平均
for (int i = 0; i < 30; i++) {
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
    for (int j = 0; j < 20; j++) {
        mpu_dmp_get_data(&pitch,&roll,&yaw);
        HAL_Delay(1);
        pitch_sum+=pitch;
        HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
    }

}
offset = pitch_sum/600.0;

效果如下：

黄线为减去偏移量之前的数据，绿线为取到的偏移量，黑线为减去偏移量之后的数据。

单词取中值处理过后的IMU数据曲线：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-vlkZBX9S-1671977201534)(https://www.houenup.com/wp-content/uploads/2022/12/图片26.jpg)]

多次取平均处理后的IMU数据曲线：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-t2bmAGvb-1671977201535)(https://www.houenup.com/wp-content/uploads/2022/12/图片27.jpg)]

3.3 伪资源不足问题

在我想要用FreeRTOS的信号量来优化程序的时候，突然发现，内存不够用了：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-mi1WdbdY-1671977201535)(https://www.houenup.com/wp-content/uploads/2022/12/图片28.jpg)]

可以看到，使用的空间达到了67K。这个问题使我非常困惑，因为FreeRTOS是一个很轻量的实时操作系统，并不会占用很多的空间。为了节省空间我开始删除一些不太必要的代码，甚至一度删除了另外两个任务线程，只剩下一个线程。

后来空间还是不够，于是去删除DMP中不必要的代码，突然使用量骤降到了56%。

经过排查之后发现，有一个printf占用了将近40%的空间。注释掉这个代码之后空间一下子变得绰绰有余。

3.4 小车不稳定（最重要）

这个原因也是最致命地原因。经过将近三天的PID的整定小车，还是很难停留在原地不动，总是会想某一个方向溜车溜走。曾经一度以为是因为PID的积分项过小导致稳态误差无法消除而倒向一边。后来经过研究网上的代码，发现并不是PID的问题，而是一开始建立的系统模型就没办法让小车保持在原地站立。

回顾一下原来的系统模型：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-QjTBuW6q-1671977201535)(https://www.houenup.com/wp-content/uploads/2022/12/图片29.png)]

在算法控制方面用的是串级PID,虽然说是两个环，但是实际上只有一个角度环，速度环是为角度环打工的。这么做存在的问题就是，车子确实是能够不倒，但是车也很难停下来。因为我们设定的系统就只保证了角度不会倾角过大而倒掉，但是至于它是在原地保持站立还是走着保持站立，它并不关心，反正保证角度为0就行了。那么假如小车现在平衡状态，控制闭环出现了微小的干扰，平衡车就会有一个方向的加速度，而此时小车角度平衡，小车平移速度没有限制，这样就可能超过PWM的幅值，导致电机无法加速了，也就没有了有效的回复力，所以小车就倒了。

如何让小车在原地站立？引入速度环（或者也可以说是位置环）。

小车的运行速度和小车的倾角是相关的，比如小车前倾，直立环要使小车向前加速，让小车保持平衡，此时小车就有速度了。如果对着速度进行闭环控制，将速度环的目标设置为0，小车就可以长期稳定平衡了。

重新设置控制系统：两个控制环：PD直立环和PI速度环。直立环控制倾角，速度环控制速度。

重新设置控制系统框图如下：

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-aW3uFnig-1671977201535)(https://www.houenup.com/wp-content/uploads/2022/12/图片30.png)]

在此基础上还可以再进行优化，在之前的角度环中可以用MPU6050读取出来的角速度来代替与Kp相乘的（ek-ek-1）。因为（ek-ek-1）就是角速度的一个间接模拟，肯定没有直接读出来的角速度值准确。

4. 链接

源码

https://gitee.com/HouEna/balance-car

演示视频

https://www.bilibili.com/video/BV1EV4y1w7V3/?spm_id_from=333.999.0.0&vd_source=e240c7dafa7cf5d1b5ebfa7d64e9b941

5. 总结

这个小车一路做下来非常的艰辛，因为这个东西在网上已经有很多成熟的产品了，于是我要求自己不要去看别人的代码，自己从0到一把这个车做出来。其中遇到了数不清的问题，花费了大量的时间，在五一假期熬了好几个通宵，熬到心律不齐……，中途一度放弃，但是我总是相信自己是有这个实力的，所以要求自己一定要做出来，于是又一遍一遍的调试，找问题、解决。这个过程中我对I2C的通讯又捋了一遍，对dmp的结算又过了一遍，对着imu的数据曲线看了一遍又一遍。总之，最后终于达到了自己可以接受的效果，在这个被折磨的过程中学到很多。

在大一的那个暑假我开始发现自己对于单片机这些东西很感兴趣便开始自学，学了一个暑假用51单片机做出了一个寻迹小车非常有成就感，这股成就感推动着我继续学习STM32单片机。大二学了一个学期，也做了一些小玩意，但是我还是感觉自己对于STM32并没有了解的那么透彻。于是便总是想寻找机会来更加精进自己的能力。

平衡小车的项目是我从大二一开学就一直想要做的，大二上学期没做出来放弃了，现在总结下来当时的能力确实不足以支撑我从零开始做成这个项目。当时连PID是什么都不知道，只是听说做平衡小车要用到PID。买的轮子也是非常的拉跨，两边的轮子发一样的PWM波转速都不一样，不会从编码器读数据一度以为是轮子坏了，于是联系淘宝店家，在店家一步步教我如何使用示波器之后验证了轮子没坏。

也正是在大二上学期制作平衡小车的时候被实验室的学长拉去参加了RM ，虽然在RM中没有做出什么贡献，甚至是在关键的时候起到了一些副作用，但是在RM中还是学到了一些东西，掌握了一些工具的使用，知道了，哦，原来单片机是可以上系统的。

总之，这个平衡小车，这个被大家做烂了的平衡小车，是我学习单片机或者是嵌入式路上真正意义上的催化剂。在写这篇博客的时候已经是22年的12月25日了，做这个小车的时间是5月份了，转眼过去七个月了，其中的一些细节或者说很多东西我都变得模糊了。但是在看我做这个小车的时候留下来的积累的资料的时候，我真的看到了当时我的热爱。大二的下半学期，我一直在为实习做着准备，学习通信协议、饶有兴致地去看FreeRtos的原理。直到7月份，开始暑假，实习路一路坎坷。开始实习时被告知实习期太短，不适合做嵌入式，被调到软件测试岗；一个人寄居在同学租的房子里（这里由衷地感谢我地这位同学），因为疫情而不能去公司办公；因为房租到期，一个人搬家，搬去另一个新的房子。期间高中舍友来海南游玩被困，和我一起住了几天，在他离开后我已经几乎无法面对一个人地房间；到最后甚至精神有些恍惚总觉得背后的门会在下一秒被突然地打开。

之后便是疫情，延期开学，直到九月份再次宣布延期到国庆之后，我便定了最近的机票飞回了家。于是，开始了摆烂的生活。在几个月的高强度折磨之后的摆烂让我如释重负，而写这篇博客的原因就是在摆烂到产生恐惧感之后，我要和过去的自己进行告别了。

希望一年后以新的身份与面孔再见我的热爱。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)