当前位置：首页 > news >正文

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

news 来源：原创 2024/9/20 9:38:08

一、编码电机驱动

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

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

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

1、PWM设置：

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

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输出的通道。

// 设置电机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、电机方向设置

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

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，驱动左轮。

// 电机前进控制函数
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

#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

#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