24年电赛——自动行驶小车（H题）MSPM0G3507-编码电机驱动与通用PID

一、编码电机驱动

编码电机的详情可以查看此篇文章：

stm32平衡小车--（1）JGB-520减速电机+tb6612（附测试代码）_jgb520-CSDN博客

简单来说，编码电机的驱动主要是给一个 PWM 和一个正负级就能驱动。PWM 的大小决定转速快慢，电机的两个电源正负极不同决定了旋转的方向不同。

1、PWM设置：

PWM 的值可以在图形化配置界面进行配置，同时也可以通过如下函数进行设置：第一个参数是定时器，第二个是捕获比较值，第三个是通道。

cs 复制代码

void DL_Timer_setCaptureCompareValue(
    GPTIMER_Regs *gptimer, uint32_t value, DL_TIMER_CC_INDEX ccIndex);

如下是设置占空比的一个函数。第一个参数是捕获比较值，第二个参数是设置通道，我是用一个定时器输出两路 PWM，所以需要两个通道。因为设置的是下降沿触发，系统主频是80MHz，进行了10000分频，所以计数值就是8000。duty = 7999 - 7999 * (duty/3200.0);就可以将捕获比较值从8000映射到3200。然后下面 if 来判断设置PWM输出的通道。

cs 复制代码

// 设置电机PWM占空比，取值范围为3200~0，对应占空比为100%~0%
void Motor_SetPwm(float duty,uint8_t channel) {

    duty = 7999 - 7999 * (duty/3200.0);

    if(channel==0){
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST , duty, DL_TIMER_CC_0_INDEX );  // 设置定时器捕获比较值
    }else {
        DL_TimerA_setCaptureCompareValue(PWM_MOTOR_INST , duty, DL_TIMER_CC_1_INDEX );  // 设置定时器捕获比较值
    }
}

PWM 图形化配置：

2、电机方向设置

此函数与上面的参数一致。这两个函数分别用来设置引脚低电平和高电平。

cs 复制代码

DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
DL_GPIO_setPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);

下面函数时根据驱动电机。duty 值为正，电机正转，duty 值为负，电机反转；channel 值为0，驱动右轮，channel 值为1，驱动左轮。

cs 复制代码

// 电机前进控制函数
void Set_Speed(float duty, uint8_t channel) {
    if(channel==0){     //判断左右轮
        if(duty>0){     //判断正反转
            Motor_SetPwm(duty,channel);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
            DL_GPIO_setPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
        }else if(duty<0){
            Motor_SetPwm(-duty,channel);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
            DL_GPIO_setPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
        }else {
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL1_PORT, GPIO_MOTOR_PIN_FL1_PIN);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FL2_PORT, GPIO_MOTOR_PIN_FL2_PIN);
        }
    }else {
        if(duty>0){
            Motor_SetPwm(duty,channel);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
            DL_GPIO_setPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
        }else if(duty<0){
            Motor_SetPwm(-duty,channel);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
            DL_GPIO_setPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
        }else {
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR1_PORT, GPIO_MOTOR_PIN_FR1_PIN);
            DL_GPIO_clearPins(GPIO_MOTOR_PIN_FR2_PORT, GPIO_MOTOR_PIN_FR2_PIN);
        }
    }
}

二、通用PID

其中增量式 PID 不需要初始阈值，其输出主要是与上次误差与上上次误差有关，适合调节稳定速度；

位置式 PID 需要初始阈值，其输出的结果与历史的状态有关，适合将速度维持在某个定值。

pid.c

cs 复制代码

#include "pid.h"
#include <math.h>

// 初始化PID结构体
void PID_Init(PID *s_PID, PID_VAR_TYPE set_point, PID_VAR_TYPE Proportion,
              PID_VAR_TYPE Integral, PID_VAR_TYPE Derivative)
{
    // 初始化PID参数
    s_PID->SetPoint = set_point;            // 设定目标值
    s_PID->Proportion = Proportion;         // 比例系数
    s_PID->Integral = Integral;             // 积分系数
    s_PID->Derivative = Derivative;         // 微分系数
    s_PID->Error = 0;                       // 当前误差
    s_PID->LastError = 0;                   // 上一次误差
    s_PID->PrevError = 0;                   // 上上次误差
    s_PID->SumError = 0;                    // 总误差
    s_PID->LastResult = 0;                  // 上一次计算结果
    s_PID->Result = 0;                      // 当前计算结果
    s_PID->OutMax = DEFAULT_PID_OUT_MAX;    // 输出上限
    s_PID->OutMin = DEFAULT_PID_OUT_MIN;    // 输出下限
    s_PID->IntegralMax = DEFAULT_PID_INTEGRAL_OUT_MAX;  // 积分输出上限
    s_PID->IntegralMin = DEFAULT_PID_INTEGRAL_OUT_MIN;  // 积分输出下限
}

// 设置PID的目标值
void PID_SetPoint(PID *s_PID, PID_VAR_TYPE set_point)
{
    s_PID->SetPoint = set_point;
}

// 设置PID的输出范围
void PID_SetOutRange(PID *s_PID, PID_VAR_TYPE outMax, PID_VAR_TYPE outMin)
{
    s_PID->OutMax = outMax;
    s_PID->OutMin = outMin;
}

// 设置PID的积分输出范围
void PID_SetIntegralOutRange(PID *s_PID, PID_VAR_TYPE outMax, PID_VAR_TYPE outMin)
{
    s_PID->IntegralMax = outMax;
    s_PID->IntegralMin = outMin;
}

// 增量型PID计算
PID_VAR_TYPE Increment_PID_Cal(PID *s_PID, PID_VAR_TYPE now_point)
{
    s_PID->LastResult = s_PID->Result;   // 保存上一次的计算结果

    // 计算误差
    s_PID->Error = s_PID->SetPoint - now_point;

    // PID计算
    s_PID->Result =
        s_PID->LastResult +
        s_PID->Proportion * (s_PID->Error - s_PID->LastError) +  // 比例项
        s_PID->Integral * s_PID->Error +                          // 积分项
        s_PID->Derivative * (s_PID->Error - 2 * s_PID->LastError + s_PID->PrevError);  // 微分项

    s_PID->PrevError = s_PID->LastError;   // 更新上上次误差
    s_PID->LastError = s_PID->Error;       // 更新上一次误差

    // 限制输出范围
    if (s_PID->Result > s_PID->OutMax)
        s_PID->Result = s_PID->OutMax;
    else if (s_PID->Result < s_PID->OutMin)
        s_PID->Result = s_PID->OutMin;

    return s_PID->Result;
}

// 位置型PID计算
PID_VAR_TYPE Position_PID_Cal(PID *s_PID, PID_VAR_TYPE now_point)
{
    s_PID->LastResult = s_PID->Result;   // 保存上一次的计算结果

    // 计算误差
    s_PID->Error = s_PID->SetPoint - now_point;
    s_PID->SumError += s_PID->Error;    // 更新总误差

    // 限制积分输出范围
    PID_VAR_TYPE IOutValue = s_PID->SumError * s_PID->Integral;
    if (IOutValue > s_PID->IntegralMax)
        IOutValue = s_PID->IntegralMax;
    else if (IOutValue < s_PID->IntegralMin)
        IOutValue = s_PID->IntegralMin;

    // PID计算
    s_PID->Result =
        s_PID->Proportion * s_PID->Error +      // 比例项
        IOutValue +                            // 积分项
        s_PID->Derivative * (s_PID->Error - s_PID->LastError);  // 微分项

    s_PID->PrevError = s_PID->LastError;   // 更新上上次误差
    s_PID->LastError = s_PID->Error;       // 更新上一次误差

    // 限制输出范围
    if (s_PID->Result > s_PID->OutMax)
        s_PID->Result = s_PID->OutMax;
    else if (s_PID->Result < s_PID->OutMin)
        s_PID->Result = s_PID->OutMin;

    return s_PID->Result;
}

// 综合型PID计算
PID_VAR_TYPE PID_Cal(PID *s_PID, PID_VAR_TYPE now_point)
{
    s_PID->LastResult = s_PID->Result;   // 保存上一次的计算结果

    // 计算误差
    s_PID->Error = s_PID->SetPoint - now_point;
    s_PID->SumError += s_PID->Error;    // 更新总误差

    // 限制积分输出范围
    PID_VAR_TYPE IOutValue = s_PID->SumError * s_PID->Integral;
    if (IOutValue > s_PID->IntegralMax)
        IOutValue = s_PID->IntegralMax;
    else if (IOutValue < s_PID->IntegralMin)
        IOutValue = s_PID->IntegralMin;

    // PID计算
    s_PID->Result =
        s_PID->Proportion * (s_PID->Error + IOutValue) +   // 比例项
        s_PID->Derivative * (s_PID->Error - s_PID->LastError);  // 微分项

    s_PID->PrevError = s_PID->LastError;   // 更新上上次误差
    s_PID->LastError = s_PID->Error;       // 更新上一次误差

    // 限制输出范围
    if (s_PID->Result > s_PID->OutMax)
        s_PID->Result = s_PID->OutMax;
    else if (s_PID->Result < s_PID->OutMin)
        s_PID->Result = s_PID->OutMin;

    return s_PID->Result;
}

pid.h

cs 复制代码

#ifndef __PID_H
#define __PID_H	  

//PID输出范围宏定义
#define DEFAULT_PID_OUT_MAX               ( 3200)
#define DEFAULT_PID_OUT_MIN               (-3200)
//PID积分范围宏定义
#define DEFAULT_PID_INTEGRAL_OUT_MAX      ( 400)
#define DEFAULT_PID_INTEGRAL_OUT_MIN      (-400)

//PID变量类型宏定义
#define PID_VAR_TYPE                            float     //int

//定义结构体
typedef struct
{
    PID_VAR_TYPE SetPoint;      // 设定目标 
    PID_VAR_TYPE Proportion;    // 比例常数
    PID_VAR_TYPE Integral;      // 积分常数
    PID_VAR_TYPE Derivative;    // 微分常数 
    PID_VAR_TYPE SumError;      // 积分和
    PID_VAR_TYPE Error;         // 误差
    PID_VAR_TYPE LastError;     // 上次误差 
    PID_VAR_TYPE PrevError;     // 前一次误差   
    PID_VAR_TYPE LastResult;    // 上次计算结果
    PID_VAR_TYPE Result;        // 当前计算结果
    PID_VAR_TYPE OutMax;        // 输出限幅最大值
    PID_VAR_TYPE OutMin;        // 输出限幅最小值
    PID_VAR_TYPE IntegralMax;   // 积分限幅最大值
    PID_VAR_TYPE IntegralMin;   // 积分限幅最小值
} PID;

extern void PID_Init(PID *s_PID, PID_VAR_TYPE set_point,
                        PID_VAR_TYPE Proportion,
                        PID_VAR_TYPE Integral,
                        PID_VAR_TYPE Derivative);  //PID初始化

extern void PID_SetOutRange(PID *s_PID, PID_VAR_TYPE outMax, PID_VAR_TYPE outMin);          //设置PID输出范围
extern void PID_SetIntegralOutRange(PID *s_PID, PID_VAR_TYPE outMax, PID_VAR_TYPE outMin);  //设置PID积分范围
extern void PID_SetPoint(PID *s_PID, PID_VAR_TYPE set_point);                               //设置目标值

extern PID_VAR_TYPE Increment_PID_Cal(PID *s_PID, PID_VAR_TYPE now_point);  //增量式PID计算
extern PID_VAR_TYPE Position_PID_Cal(PID *s_PID, PID_VAR_TYPE now_point);   //位置式PID计算
extern PID_VAR_TYPE PID_Cal(PID *s_PID, PID_VAR_TYPE now_point);            //比例外置式PID

#endif