本帖最后由 beijinglaobai 于 2022-11-30 11:35 编辑
为实现完成PID控制需要使用STM32定时器的输出通道和互补输出通道共同控制引脚链接驱动器驱动电机和编码器链接STM32 MCU定时器的编码器接口来实现一个完成的驱动、反馈闭环，根据STM32MCU的数据手册可以查询定时器的各引脚的功能。
/**
******************************************************************************
* 文件名程: main.c
* 功 能:位置速度闭环控制
******************************************************************************
*/
/* 包含头文件 ----------------------------------------------------------------*/
#include "stm32f1xx_hal.h"
#include "DCMotor/bsp_BDCMotor.h"
#include "key/bsp_key.h"
#include "usart/bsp_usartx.h"
#include "encoder/bsp_encoder.h"
/* 私有类型定义 --------------------------------------------------------------*/
typedef struct
{
__IO int32_t SetPoint; //设定目标 Desired Value
__IO float SumError; //误差累计
__IO float Proportion; //比例常数 Proportional Const
__IO float Integral; //积分常数 Integral Const
__IO float Derivative; //微分常数 Derivative Const
__IO int LastError; //Error[-1]
__IO int PrevError; //Error[-2]
}PID_TypeDef;
/* 私有宏定义 ----------------------------------------------------------------*/
#define SPEEDRATIO 270
#define ENCODER_RESOLUTION 11
#define PPR ((SPEEDRATIO*ENCODER_RESOLUTION)*4) // Pulse/Round 每圈可捕获到的脉冲数
/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
#define SPD_P_DATA 15.0f // P参数
#define SPD_I_DATA 8.8f // I参数
#define SPD_D_DATA 0.0f // D参数
#define TARGET_SPEED 10.0f // 目标速度 10r/m
#define LOC_P_DATA 0.025f // P参数
#define LOC_I_DATA 0.001f // D参数
#define LOC_D_DATA 0.08f // D参数
#define TARGET_LOC (3*PPR) // 目标位置 11880 Pulse = 1r
/* 私有变量 ------------------------------------------------------------------*/
uint32_t Motor_Dir = CW; // 电机方向
__IO uint8_t Start_flag = 0; // PID 开始标志
__IO int32_t tmpPWM_DutySpd = 0;
__IO int32_t tmpPWM_Duty = 0;
__IO int32_t Sample_Pulse; // 编码器捕获值 Pulse
__IO int32_t LastSample_Pulse; // 编码器捕获值 Pulse
__IO int32_t Spd_PPS; // 速度值 Pulse/Sample
__IO float Spd_RPM; // 速度值 r/m
static __IO uint32_t uwTick; // 滴答定时器计数
/* PID结构体 */
PID_TypeDef sPID,lPID;
/* 扩展变量 ------------------------------------------------------------------*/
/* 私有函数原形 --------------------------------------------------------------*/
void PID_ParamInit(void) ;
int32_t SpdPIDCalc(float NextPoint);
int32_t LocPIDCalc(int32_t NextPoint);
/* 函数体 --------------------------------------------------------------------*/
/**
* 函数功能: 系统时钟配置
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // 外部晶振，8MHz
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9; // 9倍频，得到72MHz主时钟
HAL_RCC_OscConfig(&RCC_OscInitStruct);
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // 系统时钟：72MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // AHB时钟： 72MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; // APB1时钟：36MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; // APB2时钟：72MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2);
// HAL_RCC_GetHCLKFreq()/1000 1ms中断一次
// HAL_RCC_GetHCLKFreq()/100000 10us中断一次
// HAL_RCC_GetHCLKFreq()/1000000 1us中断一次
HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); // 配置并启动系统滴答定时器
/* 系统滴答定时器时钟源 */
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* 系统滴答定时器中断优先级配置 */
HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}
/**
* 函数功能: 主函数.
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
int main(void)
{
/* 复位所有外设，初始化Flash接口和系统滴答定时器 */
HAL_Init();
/* 配置系统时钟 */
SystemClock_Config();
/* 按键 初始化 */
KEY_GPIO_Init();
/* 串口通信配置初始化 */
MX_USARTx_Init();
/* 编码器初始化及使能编码器模式 */
ENCODER_TIMx_Init();
/* 配置定时器脉冲输出 */
BDCMOTOR_TIMx_Init();
/* 设置电机速度 */
PWM_Duty = BDCMOTOR_DUTY_ZERO;
SetMotorSpeed(PWM_Duty);
/* 设置电机方向 */
SetMotorDir(1);
HAL_Delay(1000);
/* PID 参数初始化 */
PID_ParamInit();
/* 无限循环 */
while (1)
{
if(KEY1_StateRead()==KEY_DOWN)
{
PID_ParamInit();
PWM_Duty = 0;
SetMotorSpeed(PWM_Duty); // 0%
SetMotorStart();
HAL_Delay(500);
Start_flag = 1;
}
if(KEY2_StateRead()==KEY_DOWN)
{
Start_flag = 0;
HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
}
}
}
/**
* 函数功能: 系统滴答定时器中断回调函数
* 输入参数: 无
* 返 回 值: 无
* 说 明: 每一秒读取编码器数值,计算速度,并打印到串口
*/
void HAL_SYSTICK_Callback(void)
{
uwTick++;
if((uwTick % 50) == 0)
{
// /* 获取当前位置值,编码器4倍频之后的数值 */
Sample_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
/* 计算PID结果 */
if(Start_flag == 1)
{
tmpPWM_DutySpd = LocPIDCalc(Sample_Pulse);
/* 设定速度环的目标值 */
if(tmpPWM_DutySpd >= TARGET_SPEED)
tmpPWM_DutySpd = TARGET_SPEED;
if(tmpPWM_DutySpd <= -TARGET_SPEED)
tmpPWM_DutySpd = -TARGET_SPEED;
}
}
/* 速度环周期50ms */
if(uwTick % 50 == 0)
{
/* 获得当前速度 */
Sample_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
Spd_PPS = Sample_Pulse - LastSample_Pulse;
LastSample_Pulse = Sample_Pulse ;
/* 11线编码器,270减速比,一圈脉冲信号是11*270*4 PPR */
Spd_RPM = ((((float)Spd_PPS/(float)PPR)*20.0f)*(float)60);//单位是rpm
//
/* 计算PID结果 */
if(Start_flag == 1)
{
sPID.SetPoint = tmpPWM_DutySpd;
PWM_Duty = SpdPIDCalc(Spd_RPM);
if(PWM_Duty < 0)
{
Motor_Dir = CW;
BDDCMOTOR_DIR_CW();
PWM_Duty = -PWM_Duty;
}
else
{
Motor_Dir = CCW;
BDDCMOTOR_DIR_CCW();
}
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty );
}
printf("LOC:%d Sped: %2.2f r/m \n",Sample_Pulse,Spd_RPM );
}
}
/********************* PID参数初始化 ********************************/
/**
* 函数功能: PID参数初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void PID_ParamInit()
{
Sample_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
if(Sample_Pulse != 0)
{
Sample_Pulse = 0;
__HAL_TIM_SET_COUNTER(&htimx_Encoder,0);
}
sPID.LastError = 0; // Error[-1]
sPID.PrevError = 0; // Error[-2]
sPID.SumError = 0;
sPID.Proportion = SPD_P_DATA; // 比例常数 Proportional Const
sPID.Integral = SPD_I_DATA; // 积分常数 Integral Const
sPID.Derivative = SPD_D_DATA; // 微分常数 Derivative Const
sPID.SetPoint = TARGET_SPEED; // 设定目标Desired Value
lPID.LastError = 0; // Error[-1]
lPID.PrevError = 0; // Error[-2]
lPID.SumError = 0;
lPID.Proportion = LOC_P_DATA; // 比例常数 Proportional Const
lPID.Integral = LOC_I_DATA; // 积分常数 Integral Const
lPID.Derivative = LOC_D_DATA; // 微分常数 Derivative Const
lPID.SetPoint = TARGET_LOC; // 设定目标Desired Value
}
/******************** 电流闭环 PID 控制设计 ************************************/
/**
* 函数名称：速度闭环PID控制设计
* 输入参数：当前控制量
* 返 回 值：目标控制量
* 说 明：无
*/
int32_t SpdPIDCalc(float NextPoint)
{
float iError,dError;
iError = sPID.SetPoint - NextPoint; //偏差
if((iError<0.2f )&& (iError>-0.2f))
iError = 0.0f;
sPID.SumError += iError; //积分
/* 设定积分上限 */
if(lPID.SumError >= TARGET_SPEED*10)
lPID.SumError = TARGET_SPEED*10;
if(lPID.SumError <= -TARGET_SPEED*10)
lPID.SumError = -TARGET_SPEED*10;
dError = iError - sPID.LastError; //微分
sPID.LastError = iError;
return (int32_t)(sPID.Proportion * (float)iError //比例项
+ sPID.Integral * (float)sPID.SumError //积分项
+ sPID.Derivative * (float)dError); //微分项
}
/**
* 函数名称：位置闭环PID控制设计
* 输入参数：当前控制量
* 返 回 值：目标控制量
* 说 明：无
*/
int32_t LocPIDCalc(int32_t NextPoint)
{
int32_t iError,dError;
iError = lPID.SetPoint - NextPoint; //偏差
/* 设定闭环死区 */
if((iError >= -50) && (iError <= 50))
{
iError = 0;
lPID.SumError /= 2.0;
}
/* 积分分离 */
if((iError >= -1000) && (iError <= 1000))
{
lPID.SumError += iError; //积分
/* 设定积分上限 */
if(lPID.SumError >= 1000)
lPID.SumError = 1000;
if(lPID.SumError <= -1000)
lPID.SumError = -1000;
}
dError = iError - lPID.LastError; //微分
lPID.LastError = iError;
return (int32_t)(lPID.Proportion * (float)iError //比例项
+ lPID.Integral * (float)lPID.SumError //积分项
+ lPID.Derivative * (float)dError); //微分项
}
| 
楼主
标题置顶
标题高亮
