【STM32】【HAL库】平衡小车(PID实战2)(二)软件设计

最新推荐文章于 2024-05-17 17:01:15 发布

Hz1213825

最新推荐文章于 2024-05-17 17:01:15 发布

阅读量3.5k

点赞数 14

分类专栏：实用制作平衡小车 STM32_HAL库文章标签： stm32 单片机 arm

本文链接：https://blog.csdn.net/m0_57585228/article/details/127140318

版权

STM32_HAL库同时被 3 个专栏收录

20 篇文章 26 订阅

订阅专栏

实用制作

12 篇文章 3 订阅

订阅专栏

平衡小车

3 篇文章 2 订阅

订阅专栏

概述

平衡车作为经典PID设计项目,非常热门,也非常适合 PID/单片机初学的项目

没有看起来那么难,愿每个人都可做出自己的平衡车

本系列文章,从硬件到软件,带领大家制作平衡小车

分为三节

本节是硬件设计部分

项目目标

直立,可以在正常程度的干扰下保持不倒

在保持直立的同时,小车接近静止,尽量不左右旋转

项目架构

首先是所有外设的初始化

主要是定时器,GPIO,和MPU6050及DMP的初始化

之后再主循环里调用读取陀螺仪(角加速度)和欧拉角(这是因为通信速率的问题,不能放入定时器中断函数里进行)

在定时器中断(5ms)里读取编码器数据,进行PID计算,并将PWM给到电机执行

HAL初始化

使用的是STM32C6T6

毕竟价格便宜(现在3r以下)

定时器

需要3个定时器实现4种功能

PWM输出

使用高级定时器 TIM1 产生4路PWM

设置频率为14.4KHz(不分频72MHz频率最大装载值5000)

自动装载,其他的默认设置即可

在这里插入图片描述

编码器

有两个电机编码器需要计数

因此使用TIM2和TIM3设置为编码器模式

设置为双边沿计数(4倍频),其他全部默认即可

在这里插入图片描述

定时器中断

PID需要固定时间进行计算

但是STM32C6只有3个定时器,因此使用PWM的定时器产生中断,通过计数定时出5ms来给PID

开启溢出中断

在这里插入图片描述

I2C

使用软件I2C,也就是2个GPIO

设置为开漏上拉输出,最高等级

在这里插入图片描述

时钟设置

使用外部12MHz晶振,经过PLL后将系统频率设为72MHz

在这里插入图片描述

串口

作为DEBUG的手段

在这里插入图片描述

全景图

在这里插入图片描述

基础设置和驱动

定时器设置

TIM1 的HAL初始化函数里

开启四路PWM输出和定时器中断

HAL_TIM_Base_Start_IT(&htim1);
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);
HAL_TIM_Base_Start(&htim1);

TIM2/TIM3 的HAL初始化函数里

开启定时器作为编码器模式

详情见这篇文章,传送门

HAL_TIM_Encoder_Start(&htim2, TIM_CHANNEL_ALL);
HAL_TIM_Base_Start_IT(&htim2);

HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
HAL_TIM_Base_Start_IT(&htim3);

使用TIM1作为定时器中断源产生5ms左右(不需要特别准)的中断

这是定时器中断回调函数,计数72次运行PID计算和编码器计算

uint16_t T_1 = 0;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if (htim == &htim1)
  {
    T_1++;
    if (T_1 == 72)
    { //每5ms计算一次PID
      T_1 = 0;
     
    }
  }
}

获取转速

读取定时器的计数值,并清空

输出计算好的转速(这里没有速度控制,但输出脉冲数就行了)

正负代表正反转

#define MY_L 1
#define MY_R 0

/**
 * @brief 获取转速
 * @param R_L:左右
 * @return 转速
 * @author HZ12138
 * @date 2022-08-14 23:17:08
 */
float Get_Speed(uint8_t R_L)
{
    int16_t zj;
    float Speed;
    if (R_L == MY_L)
    {
        zj = -__HAL_TIM_GetCounter(&htim2);
        __HAL_TIM_SetCounter(&htim2, 0);
    }
    else if (R_L == MY_R)
    {
        zj = __HAL_TIM_GetCounter(&htim3);
        __HAL_TIM_SetCounter(&htim3, 0);
    }
    // Speed = (float)zj / (4 * 15 * 34) * 200 * 60;
    Speed = zj;
    return Speed;
}

设置电机转速

输入左右和PWM的值(-5000-5000)设置PWM给电机

/**
 * @brief 设置控制电机的PWM
 * @param R_L:左右
 * @param PWM_Val:PWM值(正值正转,负值反转)
 * @return 无
 * @author HZ12138
 * @date 2022-08-14 23:15:36
 */
void Set_Motor_PWM(uint8_t R_L, int16_t PWM_Val)
{
    PWM_Amplitude_Limit(&PWM_Val);
    if (R_L == MY_L)
    {
        if (PWM_Val > 0)
        {
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, PWM_Val);
            __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, -PWM_Val);
        }
    }
    else if (R_L == MY_R)
    {
        if (PWM_Val > 0)
        {
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, 0);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, PWM_Val);
        }
        else
        {
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_3, -PWM_Val);
            __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_4, 0);
        }
    }
}

MPU6050

使用DMP库解算

详情见这个博客,传送门

使用的函数是

MPU_Get_Gyroscope(&gyro_x, &gyro_y, &gyro_z);       //读取角加速度
mpu_dmp_get_data(&pitch, &roll, &yaw);              //读取DMP解算的欧拉角
MPU_Init();     // mpu6050初始化
mpu_dmp_init(); // DMP初始化

千万注意软件I2C的通信速率,读取的频率大于100Hz,两次读取的时间间隔<10ms

作者踩过的大坑

PID设计(核心)

目标有3个

直立(直立环),静止(速度环),不转动(转向环)

分别使用3个PID控制器来控制

级联为并级

框图如下

在这里插入图片描述

也就是三个PID控制器分别输出PWM给电机

注意:转向环给左右两个电机的PWM值互为相反数,其他两环给两个电机的PWM值是相同的

直立环

直立环是让小车保持直立的关键,要求快速响应(不能到已经倒下了才响应吧)

因此使用PD控制(P使其快速响应,D抑制超调)

现在我们看一下需要怎么做才能让小车保持平衡

这是保持平衡的状态(侧面)

在这里插入图片描述

现在小车将要向某一方向倒下

那就要电机向同向旋转,也就是小车会向箭头方向运动,这样才会让小车保持平衡

可以自己用手拿一个物体实验一下

在这里插入图片描述

到了这里应该已经明白了怎么让小车保持直立了

下面我们来实现这个操控过程

首先需要知道当前的姿态,也就是与小车轮子在一条直线上的欧拉角(本作者的硬件是pitch)

一般是(pitch/roll),应该没人竖直安装PCB吧

这也就是PID计算中的当前值

目标值是小车在无外力的条件下能保持最长时间直立的欧拉角

也就是很多教程所说的机械中值

这个需要实测,PID调参时细说

P(比例控制部分)已经解决了,下面解决D(微分部分)

在直立环中微分的意义是对欧拉角求导,结果就是角加速度

这样精度比自行计算要高,还节省单片机算力

所有整个直立环代码如下

float PID_Upright_Kp = 300; //直立环Kp
float PID_Upright_Kd = 1.5; //直立环Kd
/**
 * @brief 直立环(位置式PD)
 * @param expect:期望角度
 * @param angle:真实角度
 * @param gyro:真实角速度
 * @return PWM
 * @author HZ12138
 * @date 2022-08-16 23:17:39
 */
int PID_Upright(float expect, float angle, float gyro)
{
    int PWM_out;
    PWM_out = PID_Upright_Kp * (angle - expect) + PID_Upright_Kd * gyro;
    return PWM_out;
}

求出偏差,让角加速度作为微分项,加权求和输出即可

速度环

单纯直立环无法使小车保持接近直立

会随着小车运动而向一方加速倒去,直到倒下,或者出现来回震荡

所以使用速度环来改善这个现象

速度环本质上就是让速度始终保持一个速度附近,也就是抑制单直立环出现的持续加速

但是要注意的是,在整个项目中都是以直立环为主,其他的都是干扰

速度环的输入是两个电机编码器输入的平均值(加上转向环后两个编码器输出会有较大区别)

期望速度在一般情况下是0,也就是静止,接收来自遥控(本项目没有)后修改期望速度来遥控

速度环控制器不能响应过快(过快会影响直立环)

这样P的值就不能过大,但P如果较小会有很大概率出现静态误差,因此引入I(积分)

所以速度环是PI控制器

输入之后先计算出误差

之后对误差进行低通滤波(根据传递函数算出来的)(避免过大影响直立环)

再计算积分,进行积分限幅,在超过某个值时情况积分

在对积分和比例项进行加权求和就行了

float PID_Speed_Kp = 200;       //速度环Kp
float PID_Speed_Ki = 200 / 200; //速度环Ki
/**
 * @brief 速度环(位置式PI)
 * @param expect:期望速度
 * @param speed:当前速度
 * @return PWM
 * @author HZ12138
 * @date 2022-08-21 13:06:59
 */
int PID_Speed(int16_t expect, int16_t speed)
{
    static int Speed_Integral, Speed_Err_Last;
    int PWM_out, Speed_Err;
    float a = 0.7;

    Speed_Err = speed - expect;                           //误差计算
    Speed_Err = (1 - a) * Speed_Err + a * Speed_Err_Last; //低通滤波
    Speed_Err_Last = Speed_Err;                           //更新上次误差

    Speed_Integral += Speed_Err; //积分计算

    if (Speed_Integral > 10000)
        Speed_Integral = 0;
    if (Speed_Integral < -10000)
        Speed_Integral = 0; //积分限幅

    PWM_out = PID_Speed_Kp * Speed_Err + PID_Speed_Ki * Speed_Integral; //计算输出
    return PWM_out;
}

有了直立环和速度环,小车就能保持长时间平衡了

转向环

这个是为了解决在小车静止或运动时左右旋转的问题

这个问题的起因是两个电机(驱动)的一致性不好

在常规车模的跑直线中也常用

转向环有两个目标

1是在没有遥控的情况下保持方向不变

2是接收数据后向指定方向旋转

因此使用PD控制

D是用于始终阻碍变化方向

P是用于将方向设为指定方向

输入的是目标和当前的欧拉角(yaw),还有角加速度z方向的

进行低通滤波和加权求和后输出即可

float PID_Steering_Kp = 0; //转向环Kp
float PID_Steering_Kd = 5; //转向环Kd
/**
 * @brief 转向环(位置式PD)
 * @param expect:期望转向的角度
 * @param angle:真实的角度(一般是yaw)
 * @param gyro:真实角加速度(一般是gyro_z)
 * @return PWM
 * @author HZ12138
 * @date 2022-08-21 13:07:02
 */
int PID_Steering(int16_t expect, float angle, float gyro)
{
    int PWM_out;
    float a = 0.7;
    static float gyro_Last;
    gyro = (1 - a) * gyro + a * gyro_Last; //低通滤波
    PWM_out = PID_Steering_Kp * (angle - expect) + PID_Steering_Kd * gyro;
    return PWM_out;
}

总程序

每5ms计算一次PID

获取转速,并压入三级PID中

经过限幅后和死区屏蔽后输出给电机即可

#define PWM_Lim 500
#define Z_Static_Deviation (-16)
uint8_t init_OK = 0;
float pitch, roll, yaw;
short gyro_x, gyro_y, gyro_z;
int16_t Speed = 0;
uint16_t T_1 = 0;
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  float Speed_L, Speed_R;
  int16_t PWM_Upright = 0,
          PWM_Speed = -4,
          PWM_Steering = 0,
          PID_PWM_R = 0,
          PID_PWM_L = 0;
  if (htim == &htim1)
  {
    T_1++;
    if (T_1 == 72)
    { //每5ms计算一次PID
      T_1 = 0;
      if (init_OK == 1)
      {

        Speed_L = Get_Speed(MY_L);
        Speed_R = Get_Speed(MY_R);
        Speed = Speed_L + Speed_R; //获取转速

        PWM_Upright = PID_Upright(-3.5, pitch, gyro_y);                    //直立环
        PWM_Speed = PID_Speed(0, Speed);                                   //速度环
        PWM_Steering = PID_Steering(10, yaw, gyro_z + Z_Static_Deviation); //转向环

        //计算并级PID
        PID_PWM_R = PWM_Upright + PWM_Speed - PWM_Steering;
        PID_PWM_L = PWM_Upright + PWM_Speed + PWM_Steering;

        // PWM低占空比低时不给电机(避免堵转异响)
        if (PID_PWM_L > PWM_Lim || PID_PWM_L < -PWM_Lim)
          Set_Motor_PWM(MY_L, PID_PWM_L);
        if (PID_PWM_R > PWM_Lim || PID_PWM_R < -PWM_Lim)
          Set_Motor_PWM(MY_R, PID_PWM_R);
      }
    }
  }
}

附加功能

拿起检测

通过实验可以发现,当拿起小车时,因为所有的反馈没法作用到轮子,

因此有一个小角度的倾斜时轮子就会以较大的转速向一方前进

我们,可以检测占空比,当一段时间(建议500ms)大于某个值时(正常情况不会出现的),关闭电机,认为被拿起了

这个函数是被5ms调用一次

if (init_OK == 1)
  {
    if (PWM_Upright + PWM_Speed > 3000)
      T_2++;
    else
    {
      if (pitch > 40 || pitch < -40)
        T_2++;
      else
        T_2 = 0;
    }

    if (T_2 > 100)
    {
      init_OK = 0;
      Set_Motor_PWM(MY_R, 0);
      Set_Motor_PWM(MY_L, 0);
    }
  }

放下检测

放下检测我设置的是,让小车保持接近直立状态几秒后,重启程序,开始PID运行以及电机启动

这样我们只需要检测欧拉角和角加速度,让这两个值在一段时间内保持接近直立的状态就行了

这个函数是被5ms调用一次

 if (init_OK == 0)
  {
    if (pitch < 5 + Mechanical_Median && pitch > -5 + Mechanical_Median && gyro_y < 5 && gyro_y > -5)
    {
      T_3++;
    }

    if (T_3 > 400)
    {
      __set_FAULTMASK(1); //关中断
      NVIC_SystemReset(); //复位
    }

合

void Pick_Up_Test()
{
  if (init_OK == 1)
  {
    if (PWM_Upright + PWM_Speed > 3000)
      T_2++;
    else
    {
      if (pitch > 40 || pitch < -40)
        T_2++;
      else
        T_2 = 0;
    }

    if (T_2 > 100)
    {
      init_OK = 0;
      Set_Motor_PWM(MY_R, 0);
      Set_Motor_PWM(MY_L, 0);
    }
  }
  if (init_OK == 0)
  {
    if (pitch < 5 + Mechanical_Median && pitch > -5 + Mechanical_Median && gyro_y < 5 && gyro_y > -5)
    {
      T_3++;
    }

    if (T_3 > 400)
    {
      __set_FAULTMASK(1); //关中断
      NVIC_SystemReset(); //复位
    }
  }
}

成品

GitHub

百度网盘

Hz1213825

关注

14
点赞
踩
94

收藏

觉得还不错? 一键收藏
1
评论
【STM32】【HAL库】平衡小车(PID实战2)(二)软件设计

平衡车作为经典PID设计项目,非常热门,也非常适合 PID/单片机初学的项目没有看起来那么难,愿每个人都可做出自己的平衡车本系列文章,从硬件到软件,带领大家制作平衡小车分为三节硬件设计软件设计PID调参本节是硬件设计部分。
复制链接

扫一扫