1.为什么要用多闭环?
比起单闭环多闭环增加系统的稳定性并且提高效率。比如:控制位置环如果采用单闭环可能误差大不太稳定。但用多闭环比如位置电流环。能很提高稳定性。相当电流环只是辅助作用。实际控制的还是位置。这就是用多闭环的好处。
2.什么是多闭环
多闭环也叫串极控制:就是采用两个控制器串联工作(位置速度,位置电流,速度电流)。如果三闭环就是三个控制器串联工作(位置,速度,电流)。
多闭环要记住两点很重要:1.就是目标做外环,辅助做内环。并且外环的输出做内环的目标值,内环的输出去控制被控制量。2.只是提高外环的目标的稳定性与效率。其它辅助环只是其辅助效果。内环:对时间要求高,实时效应需求高,需要频繁处理(采样周期短)的闭环系统。外环:对时间要求较弱,可以间隔较长时间处理(采样周期较长)的闭环系统。
总结:电机控制多闭环(三闭环)系统中:电流环作为最内环,速度环次之,位置环最外。所以,位置环输出作为速度环输入(目标速度),速度环输出作为电流环输入(目标电流)。
3.为什么电流环做内环?
其实只要知道目标是什么外环就是什么。电流是经过电感的表现值,属于一阶系统,其对系统变化的敏感度大大好于电压环。如果目标是电流,要么单闭环电流闭环。多闭环的话电流环做外环其它辅助环做内环这样效率反而变的不好。所以电流环几乎不做外环即使目标是电流。
4.多闭环的列子
1.位置速度多闭环(位置,速度都是位置PID)
请牢牢记住:外环的输出做内环的目标值,内环的输出去控制被控制量。这样代码就容易。
由于是位置速度会用到编码器,并且要用互补PWM控制电机。所以需要两个定时器(一个通用与一个高级)。
编码器.h
#ifndef __BSP_ENCODER_H__
#define __BSP_ENCODER_H__
#include "stm32f4xx_hal.h"
#include "usart/bsp_usartx.h"
#define ENCODER_TIMx TIM3
#define ENCODER_TIM_RCC_CLK_ENABLE() __HAL_RCC_TIM3_CLK_ENABLE()
#define ENCODER_TIM_RCC_CLK_DISABLE() __HAL_RCC_TIM3_CLK_DISABLE()
#define ENCODER_TIM_GPIO_CLK_ENABLE() __HAL_RCC_GPIOC_CLK_ENABLE()
#define ENCODER_TIM_CH1_PIN GPIO_PIN_6 // CH1
#define ENCODER_TIM_CH1_GPIO GPIOC
#define ENCODER_TIM_CH2_PIN GPIO_PIN_7 // CH2
#define ENCODER_TIM_CH2_GPIO GPIOC
#define TIM_ENCODERMODE_TIx TIM_ENCODERMODE_TI12 // A,B上下沿都计数(4倍频)
#define ENCODER_TIM_IRQn TIM3_IRQn
#define ENCODER_TIM_IRQHANDLER TIM3_IRQHandler
#define ENCODER_TIM_PRESCALER 0 // 预分频值为0(不分频)
#define ENCODER_TIM_PERIOD 0xFFFF // ARR 0xFFFF
#define CNT_MAX ((int32_t)65536)
extern TIM_HandleTypeDef htimx_Encoder;
extern int32_t OverflowCount ;//定时器溢出次数
void ENCODER_TIMx_Init(void);
#endif /* __ENCODER_TIM_H__ */
代码里多看里面的单行注释,可以进一步看懂代码。
编码器.c
#include "encoder/bsp_encoder.h"
int32_t OverflowCount = 0;//定时器溢出次数
/* Timer handler declaration */
TIM_HandleTypeDef htimx_Encoder;
/* Timer Encoder Configuration Structure declaration */
TIM_Encoder_InitTypeDef sEncoderConfig;
void ENCODER_TIMx_Init(void)
{
ENCODER_TIM_RCC_CLK_ENABLE();
htimx_Encoder.Instance = ENCODER_TIMx;
htimx_Encoder.Init.Prescaler = ENCODER_TIM_PRESCALER;
htimx_Encoder.Init.CounterMode = TIM_COUNTERMODE_UP;
htimx_Encoder.Init.Period = ENCODER_TIM_PERIOD;
htimx_Encoder.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1;
sEncoderConfig.EncoderMode = TIM_ENCODERMODE_TIx; a// 编码器模式
sEncoderConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
sEncoderConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
sEncoderConfig.IC1Prescaler = TIM_ICPSC_DIV1;
sEncoderConfig.IC1Filter = 0;
sEncoderConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
sEncoderConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
sEncoderConfig.IC2Prescaler = TIM_ICPSC_DIV1;
sEncoderConfig.IC2Filter = 0;
__HAL_TIM_SET_COUNTER(&htimx_Encoder,0);
HAL_TIM_Encoder_Init(&htimx_Encoder, &sEncoderConfig);
__HAL_TIM_CLEAR_IT(&htimx_Encoder, TIM_IT_UPDATE); // 清除更新中断标志位
__HAL_TIM_URS_ENABLE(&htimx_Encoder); // 仅允许计数器溢出才产生更新中断
HAL_TIM_Base_Start_IT(&htimx_Encoder); // 使能定时器及开启更新中断
HAL_NVIC_SetPriority(ENCODER_TIM_IRQn, 0, 0); // 设置中断优先级
HAL_NVIC_EnableIRQ(ENCODER_TIM_IRQn); // 使能中断
HAL_TIM_Encoder_Start(&htimx_Encoder, TIM_CHANNEL_ALL ); // 开始编码器模式
}
/**
* 函数功能: 基本定时器硬件初始化配置
* 输入参数: htim_base:基本定时器句柄类型指针
* 返 回 值: 无
* 说 明: 该函数被HAL库内部调用
*/
void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* htim_base)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(htim_base->Instance==ENCODER_TIMx)
{
/* 基本定时器外设时钟使能 */
ENCODER_TIM_GPIO_CLK_ENABLE();
/* 定时器通道1功能引脚IO初始化 */
GPIO_InitStruct.Pin = ENCODER_TIM_CH1_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
HAL_GPIO_Init(ENCODER_TIM_CH1_GPIO, &GPIO_InitStruct);
/* 定时器通道2功能引脚IO初始化 */
GPIO_InitStruct.Pin = ENCODER_TIM_CH2_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_PULLDOWN;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
HAL_GPIO_Init(ENCODER_TIM_CH2_GPIO, &GPIO_InitStruct);
}
}
/**
* 函数功能: 定时器更新中断
* 输入参数: *htim,定时器句柄
* 返 回 值: 无
* 说 明: 编码器捕获溢出计数
*/
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim->Instance == TIM3)
{
if(__HAL_TIM_IS_TIM_COUNTING_DOWN(&htimx_Encoder))
OverflowCount--; //向下计数溢出
else
{
OverflowCount++; //向上计数溢出
}
}
}
定时器结构体每个参数的含义就没有注释很详细可以追代码看官方的注释。
高级定时器PWM.h
#ifndef __BDCMOTOR_TIM_H__
#define __BDCMOTOR_TIM_H__
#include "stm32f4xx_hal.h"
#define BDCMOTOR_TIMx TIM1
#define BDCMOTOR_TIM_RCC_CLK_ENABLE() __HAL_RCC_TIM1_CLK_ENABLE()
#define BDCMOTOR_TIM_RCC_CLK_DISABLE() __HAL_RCC_TIM1_CLK_DISABLE()
// 高级定时器的CH1 PWM 引脚
#define BDCMOTOR_TIM_CH1_GPIO_CLK_ENABLE() __HAL_RCC_GPIOA_CLK_ENABLE()
#define BDCMOTOR_TIM_CH1_PORT GPIOA
#define BDCMOTOR_TIM_CH1_PIN GPIO_PIN_8
// 高级定时器的CH1 PWMN 引脚
#define BDCMOTOR_TIM_CH1N_GPIO_CLK_ENABLE() __HAL_RCC_GPIOB_CLK_ENABLE()
#define BDCMOTOR_TIM_CH1N_PORT GPIOB
#define BDCMOTOR_TIM_CH1N_PIN GPIO_PIN_13
// PWM使能引脚
#define SHUTDOWN_GPIO_CLK_ENABLE() __HAL_RCC_GPIOH_CLK_ENABLE()
#define SHUTDOWN_PORT GPIOH
#define SHUTDOWN_PIN GPIO_PIN_6
// 驱动板有一个SD做使能控制PWM输出接在 PH6
#define ENABLE_MOTOR() HAL_GPIO_WritePin(SHUTDOWN_PORT,SHUTDOWN_PIN,GPIO_PIN_RESET)
#define SHUTDOWN_MOTOR() HAL_GPIO_WritePin(SHUTDOWN_PORT,SHUTDOWN_PIN,GPIO_PIN_SET)
#define BDCMOTOR_TIM_CC_IRQx TIM1_CC_IRQn
#define BDCMOTOR_TIM_CC_IRQxHandler TIM1_CC_IRQHandler // 输出比较中断
// 定义定时器预分频,定时器实际时钟频率为:168MHz/(BDCMOTOR_TIMx_PRESCALER+1)
#define BDCMOTOR_TIM_PRESCALER 1 // 实际时钟频率为:84MHz
#define BDCMOTOR_TIM_PERIOD 4199 // PWM频率为84MHz/(4199+1)=21KHz
#define BDCMOTOR_DUTY_ZERO (((BDCMOTOR_TIM_PERIOD+1)>>1)-1) // 0%占空比
#define BDCMOTOR_DUTY_FULL (BDCMOTOR_TIM_PERIOD-100) // 97.625%占空比 (占空比不能达不到100%)
#define BDDCMOTOR_DIR_CW() {HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);\
HAL_TIMEx_PWMN_Start(&htimx_BDCMOTOR,TIM_CHANNEL_1);}
#define BDDCMOTOR_DIR_CCW() {HAL_TIM_PWM_Start(&htimx_BDCMOTOR,TIM_CHANNEL_1);\
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);}
#define CW 1
#define CCW 0
#define BDCMOTOR_TIM_REPETITIONCOUNTER 0 // 高级定时器重复计数寄存器值
extern TIM_HandleTypeDef htimx_BDCMOTOR;
extern __IO int16_t PWM_Duty;
void BDCMOTOR_TIMx_Init(void);
#endif /* __BDCMOTOR_TIM_H__ */
高级定时器PWM.c
#include "DCMotor/bsp_BDCMotor.h"
TIM_HandleTypeDef htimx_BDCMOTOR;
__IO int16_t PWM_Duty=BDCMOTOR_DUTY_ZERO;// 占空比:PWM_Duty/BDCMOTOR_TIM_PERIOD*100%
* 函数功能: 基本定时器硬件初始化配置
* 输入参数: htim_base:基本定时器句柄类型指针
* 返 回 值: 无
* 说 明: BDCMOTOR相关GPIO初始化配置,该函数被HAL库内部调用.
*/
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef *htim)
{
/* BDCMOTOR相关GPIO初始化配置 */
if(htim == &htimx_BDCMOTOR)
{
GPIO_InitTypeDef GPIO_InitStruct;
/* 引脚端口时钟使能 */
__HAL_RCC_GPIOE_CLK_ENABLE();
BDCMOTOR_TIM_CH1_GPIO_CLK_ENABLE();
BDCMOTOR_TIM_CH1N_GPIO_CLK_ENABLE();
SHUTDOWN_GPIO_CLK_ENABLE();
/* BDCMOTOR输出脉冲控制引脚IO初始化 */
GPIO_InitStruct.Pin = BDCMOTOR_TIM_CH1_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
HAL_GPIO_Init(BDCMOTOR_TIM_CH1_PORT, &GPIO_InitStruct);
GPIO_InitStruct.Pin = BDCMOTOR_TIM_CH1N_PIN;
HAL_GPIO_Init(BDCMOTOR_TIM_CH1N_PORT, &GPIO_InitStruct);
__HAL_RCC_GPIOE_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_11;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
GPIO_InitStruct.Pin = SHUTDOWN_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = 0;
HAL_GPIO_Init(SHUTDOWN_PORT, &GPIO_InitStruct);
/* 使能电机控制引脚 */
ENABLE_MOTOR();
}
}
/**
* 函数功能: BDCMOTOR定时器初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void BDCMOTOR_TIMx_Init(void)
{
TIM_ClockConfigTypeDef sClockSourceConfig; // 定时器时钟
TIM_OC_InitTypeDef sConfigOC;
TIM_BreakDeadTimeConfigTypeDef sBDTConfig; // 定时器死区时间比较输出
/* 基本定时器外设时钟使能 */
BDCMOTOR_TIM_RCC_CLK_ENABLE();
/* 定时器基本环境配置 */
htimx_BDCMOTOR.Instance = BDCMOTOR_TIMx; // 定时器编号
htimx_BDCMOTOR.Init.Prescaler = BDCMOTOR_TIM_PRESCALER; // 定时器预分频器
htimx_BDCMOTOR.Init.CounterMode = TIM_COUNTERMODE_UP; // 计数方向:向上计数
htimx_BDCMOTOR.Init.Period = BDCMOTOR_TIM_PERIOD; // 定时器周期
htimx_BDCMOTOR.Init.ClockDivision=TIM_CLOCKDIVISION_DIV1; // 时钟分频
htimx_BDCMOTOR.Init.RepetitionCounter = BDCMOTOR_TIM_REPETITIONCOUNTER; // 重复计数器
/* 初始化定时器比较输出环境 */
HAL_TIM_PWM_Init(&htimx_BDCMOTOR);
/* 定时器时钟源配置 */
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; // 使用内部时钟源
HAL_TIM_ConfigClockSource(&htimx_BDCMOTOR, &sClockSourceConfig);
/* 死区刹车配置,实际上配置无效电平是高电平 */
sBDTConfig.AutomaticOutput = TIM_AUTOMATICOUTPUT_DISABLE ;
sBDTConfig.BreakPolarity = TIM_BREAKPOLARITY_LOW ;
sBDTConfig.BreakState = TIM_BREAK_DISABLE ;
sBDTConfig.DeadTime = 0 ;
sBDTConfig.LockLevel = TIM_LOCKLEVEL_OFF ;
sBDTConfig.OffStateIDLEMode= TIM_OSSI_DISABLE ;
sBDTConfig.OffStateRunMode = TIM_OSSR_ENABLE ;
HAL_TIMEx_ConfigBreakDeadTime(&htimx_BDCMOTOR,&sBDTConfig);
/* 定时器比较输出配置 */
sConfigOC.OCMode = TIM_OCMODE_PWM1; // 比较输出模式:PWM1模式
sConfigOC.Pulse = PWM_Duty; // 占空比
sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW; // 输出极性
sConfigOC.OCNPolarity = TIM_OCNPOLARITY_LOW; // 互补通道输出极性
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; // 快速模式
sConfigOC.OCIdleState = TIM_OCIDLESTATE_RESET; // 空闲电平
sConfigOC.OCNIdleState = TIM_OCNIDLESTATE_RESET; // 互补通道空闲电平
HAL_TIM_PWM_ConfigChannel(&htimx_BDCMOTOR, &sConfigOC, TIM_CHANNEL_1);
/* 启动定时器 */
HAL_TIM_Base_Start(&htimx_BDCMOTOR);
}
main.c(这个文件才是要仔细看的)
#include "stm32f4xx_hal.h"
#include "key/bsp_key.h"
#include "encoder/bsp_encoder.h"
#include "usart/bsp_usartx.h"
#include "DCMotor/bsp_BDCMotor.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; //上次误差
}PID_TypeDef;
/* 私有宏定义 ----------------------------------------------------------------*/
#define SPEEDRATIO 30 // 因为是减速型直流有刷电机 这个是减速比
#define ENCODER_RESOLUTION 11 // 电机的线数
// 加减速比后外面电机轴转一圈,内部编码器发出的脉冲数
#define PPR ((SPEEDRATIO*ENCODER_RESOLUTION)*4)
// 上面乘4的含义:编码器模式是A,B相上下沿都计数所以4倍频 (看自己编码器模式的配置)
/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
// 内环速度的PID参数
#define SPD_P_DATA 5.0f // P参数
#define SPD_I_DATA 8.5f // I参数
#define SPD_D_DATA 0.0f // D参数
#define TARGET_SPEED 10.0f // 目标速度 10r/m(每分钟10圈) 最大速度的限定
// 外环位置的PID参数
#define LOC_P_DATA 0.01f // P参数
#define LOC_I_DATA 0.0f // D参数
#define LOC_D_DATA 0.08f // D参数
#define TARGET_LOC (3*PPR) // 目标位置 转3圈
/* 私有变量 ------------------------------------------------------------------*/
__IO uint8_t Start_flag = 0; // PID 开始标志
uint32_t Motor_Dir = CW; // 电机方向
__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
__IO float du = 0; // 角度
/* 扩展变量 ------------------------------------------------------------------ */
extern __IO uint32_t uwTick;
/* PID结构体 */
PID_TypeDef sPID; // 速度PID参数结构体
PID_TypeDef lPID; // 位置PID参数结构体
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;
__HAL_RCC_PWR_CLK_ENABLE(); // 使能PWR时钟
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); // 设置调压器输出电压级别1
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // 外部晶振,8MHz
RCC_OscInitStruct.HSEState = RCC_HSE_ON; // 打开HSE
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; // 打开PLL
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; // PLL时钟源选择HSE
RCC_OscInitStruct.PLL.PLLM = 8; // 8分频MHz
RCC_OscInitStruct.PLL.PLLN = 336; // 336倍频
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // 2分频,得到168MHz主时钟
RCC_OscInitStruct.PLL.PLLQ = 7; // USB/SDIO/随机数产生器等的主PLL分频系数
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; // 系统时钟:168MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // AHB时钟: 168MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // APB1时钟:42MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // APB2时钟:84MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
HAL_RCC_EnableCSS(); // 使能CSS功能,优先使用外部晶振,内部时钟源为备用
// HAL_RCC_GetHCLKFreq()/1000 1ms中断一次
// HAL_RCC_GetHCLKFreq()/100000 10us中断一次
// HAL_RCC_GetHCLKFreq()/1000000 1us中断一次
HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000); // 配置并启动系统滴答定时器 1ms中断一次
/* 系统滴答定时器时钟源 */
HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);
/* 系统滴答定时器中断优先级配置 */
HAL_NVIC_SetPriority(SysTick_IRQn, 0, 0);
}
/**
* 函数功能: 主函数.
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
int main(void)
{
/* 复位所有外设,初始化Flash接口和系统滴答定时器 */
HAL_Init();
/* 配置系统时钟 */
SystemClock_Config();
/* 串口初始化 */
MX_USARTx_Init();
/* 按键初始化 */
KEY_GPIO_Init();
/* 编码器初始化及使能编码器模式 */
ENCODER_TIMx_Init();
/* 高级控制定时器初始化并配置PWM输出功能 */
BDCMOTOR_TIMx_Init();
/* 启动定时器通道和互补通道PWM输出 */
PWM_Duty = 0;
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty); // 0% 占空比 = 0 一开始是停止的
/* PID 参数初始化 */
PID_ParamInit();
/* 无限循环 */
while (1)
{
/* 停止按钮启动PWM只是占空比 = 0 也不会启动 */
if(KEY1_StateRead()==KEY_DOWN)
{
if( lPID.Proportion < 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,0); // 0%
Start_flag = 1; /* 启动电机标志置1 */
}
if(KEY2_StateRead()==KEY_DOWN) // 停止输出
{
SHUTDOWN_MOTOR(); // SD引脚失能
HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
}
if(KEY3_StateRead()==KEY_DOWN) // 圈数+1
{
lPID.SetPoint += PPR;
}
if(KEY4_StateRead()==KEY_DOWN) // 圈数-1
{
lPID.SetPoint -= PPR;
}
}
}
/**
* 函数功能: 系统滴答定时器中断回调函数
* 输入参数: 无
* 返 回 值: 无
* 说 明: 每隔一定的时间就执行pid算法
*/
void HAL_SYSTICK_Callback(void)
{
/* 位置环周期100ms */
if(uwTick % 100 == 0)
{
/* 获取当前位置值,编码器4倍频之后的数值 */
Sample_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder); // 100ms的脉冲个数编码器输出的总脉冲数
/* 计算PID结果 */
if(Start_flag == 1)
{
tmpPWM_DutySpd = LocPIDCalc(Sample_Pulse); // 位置PID的输出
// 多闭环外环的输出做内环的目标值 tmpPWM_DutySpd做速度的目标值但速度已经设置目标值
// 一般把这个目标值设置成最大速度目标值,所以会有如下判断
/* 设定速度环的目标值 */
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); // 50ms内编码器的计数值
Spd_PPS = Sample_Pulse - LastSample_Pulse; // 编码器增量 脉冲数
// 当前50ms编码器脉冲数减去前一次50ms编码器脉冲数 就可以知道脉冲增量
LastSample_Pulse = Sample_Pulse ; // 保存上一次编码器的计数值
/* 11线编码器,30减速比,一圈脉冲信号是11*30 *4 PPR */
// Spd_PPS / PPR 就是把增量脉冲数除以 一圈的脉冲数从而得到增量脉冲数转了多少圈
Spd_RPM = ((((float)Spd_PPS/(float)PPR)*20.0f)*(float)60); // 50ms *20 = 1s (20是化成秒) ,60的含义:化成每分钟多少转(单位是rpm)
// PPR 是一圈(360)编码器输出的脉冲数
du = ((float)Sample_Pulse / PPR)*360; // 把脉冲转成角度
/* 计算PID结果 */
if(Start_flag == 1)
{
// 外环的输出做内环的目标值,内环的输出控制控制量
// 外环的输出做内环的目标值
sPID.SetPoint = tmpPWM_DutySpd;
// 内环的输出控制控制量
// 由于速度的目标是每分钟多少转 Spd_RPM也应该化成每分钟多少转单位
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 OverflowCount:%d du = %2.2f\n",Sample_Pulse,Spd_RPM ,OverflowCount,du);
// 打印到上位机 参数1:位置目标总的脉冲数 参数2:每分钟多少转 参数3:把速度50ms总的编码器脉冲数转换为角度
}
}
/******************** PID 控制设计 ***************************/
/**
* 函数功能: PID参数初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void PID_ParamInit()
{
sPID.LastError = 0; // Error[-1]
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.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控制设计
* 输入参数:当前控制量
* 返 回 值:目标控制量
* 说 明:无
*/
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(sPID.SumError >= TARGET_SPEED*10)
sPID.SumError = TARGET_SPEED*10;
if(sPID.SumError <= -TARGET_SPEED*10)
sPID.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 = 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); //微分项
}
多看代码里的注释,位置,速度通过滴答定时器计时时间到进行采集打印的。
实验打印:
2.速度电流多闭环(两个PID都采用位置式PID)
由于多了电流所以需要加入ADC采集到的电压计算采集电流。编码器与高级定时器PWM跟上面一样。
ADC.h
#ifndef __ADC_H__
#define __ADC_H__
#include "stm32f4xx_hal.h"
#define ADCx_RCC_CLK_ENABLE() __HAL_RCC_ADC3_CLK_ENABLE()
#define ADCx_RCC_CLK_DISABLE() __HAL_RCC_ADC3_CLK_DISABLE()
#define ADCx ADC3
#define ADC_CURRENT_CHANNEL ADC_CHANNEL_8
#define ADC_OVP_IRQx ADC_IRQn
#define ADC_OVP_IRQHandler ADC_IRQHandler
#define DMAx_RCC_CLK_ENABLE() __HAL_RCC_DMA2_CLK_ENABLE()
#define ADCx_DMA_IRQx DMA2_Stream0_IRQn
#define ADCx_DMA_IRQx_Handler DMA2_Stream0_IRQHandler
#define DMAx_Stream_x DMA2_Stream0
#define DMAx_CHANNEL_x DMA_CHANNEL_2
#define ADC_CUR_GPIO_ClK_ENABLE() __HAL_RCC_GPIOF_CLK_ENABLE()
#define ADC_CUR_GPIO GPIOF
#define ADC_CUR_GPIO_PIN GPIO_PIN_10
#define VOLT_REF 3.3f // ADC参考电压
/* 根据驱动板设置 放大倍数 采样电阻 */
#define GAIN 6.8f // 放大倍数
#define SAMPLING_RES 0.01f // 采样电阻
/** 电压分辨率 = ADC(Hex) * 3.3 / 2^n * 1000(mV) 单位是mV
* STM32的ADC分辨率是n = 12bit,电机控制脉冲是20KHz,理论上采样率40Kz就可以
* 正确采集到波形计算电流,根据过采样理论:实际采样率 > 40KHz * 4^2,所以可
* 以提高分辨率到14bit.所以这里用 2^14 = 16384
*/
#define VOLT_RESOLUTION ((float)((VOLT_REF/(float)(16384))*(float)1000)) // ADC 电压分辨率,单位:0.201mV
/* 扩展变量 ------------------------------------------------------------------*/
extern ADC_HandleTypeDef hadcx;
extern DMA_HandleTypeDef hdma_adcx;
/* 函数声明 ------------------------------------------------------------------*/
void MX_ADCx_Init(void);
void MX_DMA_Init(void) ;
void SetChannelAsRank1(ADC_HandleTypeDef* hadc,uint32_t Channel);
#endif /* __ADC_H__ */
上面的一些是根据电机驱动板电路设置的,根据自己的驱动板设置。
ADC.c
#include "adc/bsp_adc.h"
ADC_HandleTypeDef hadcx;
DMA_HandleTypeDef hdma_adcx;
/**
* 函数功能: AD转换初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明:无
*/
void MX_ADCx_Init(void)
{
ADC_ChannelConfTypeDef sConfig = {0};
/* 外设时钟使能 */
ADCx_RCC_CLK_ENABLE();
hadcx.Instance = ADCx;
hadcx.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
hadcx.Init.Resolution = ADC_RESOLUTION_12B;
hadcx.Init.ScanConvMode = DISABLE;
hadcx.Init.ContinuousConvMode = ENABLE;
hadcx.Init.DiscontinuousConvMode = DISABLE;
hadcx.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadcx.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadcx.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadcx.Init.NbrOfConversion = 1;
hadcx.Init.DMAContinuousRequests = ENABLE;
hadcx.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
HAL_ADC_Init(&hadcx);
/* 配置电流采样通道 */
sConfig.Channel = ADC_CURRENT_CHANNEL; // 通道
sConfig.Offset = 0; // 偏移
sConfig.Rank = 0x01; // 转换序号
sConfig.SamplingTime = ADC_SAMPLETIME_15CYCLES; // 采样时间
HAL_ADC_ConfigChannel(&hadcx,&sConfig);
/* 初始化ADC_DMA */
MX_DMA_Init();
}
void MX_DMA_Init(void)
{
/* 使能外设时钟 */
DMAx_RCC_CLK_ENABLE();
hdma_adcx.Instance = DMAx_Stream_x;
hdma_adcx.Init.Channel = DMAx_CHANNEL_x;
hdma_adcx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_adcx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_adcx.Init.MemInc = DMA_MINC_ENABLE;
hdma_adcx.Init.PeriphDataAlignment = DMA_PDATAALIGN_HALFWORD;
hdma_adcx.Init.MemDataAlignment = DMA_MDATAALIGN_HALFWORD;
hdma_adcx.Init.Mode = DMA_CIRCULAR;
hdma_adcx.Init.Priority = DMA_PRIORITY_HIGH;
hdma_adcx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
HAL_DMA_Init(&hdma_adcx);
__HAL_LINKDMA(&hadcx,DMA_Handle,hdma_adcx); // DMA与ADC联系起来
/* 外设中断优先级配置和使能中断 */
HAL_NVIC_SetPriority(ADCx_DMA_IRQx, 1, 1);
HAL_NVIC_EnableIRQ(ADCx_DMA_IRQx);
}
/**
* 函数功能: ADC外设初始化配置
* 输入参数: hadc:AD外设句柄类型指针
* 返 回 值: 无
* 说 明: 该函数被HAL库内部调用
*/
void HAL_ADC_MspInit(ADC_HandleTypeDef* hadc)
{
GPIO_InitTypeDef GPIO_InitStruct;
if(hadc->Instance==ADCx)
{
/* AD转换通道引脚时钟使能 */
ADC_CUR_GPIO_ClK_ENABLE();
/* AD转换通道引脚初始化 */
GPIO_InitStruct.Pin = ADC_CUR_GPIO_PIN;
GPIO_InitStruct.Mode = GPIO_MODE_ANALOG; // 配置输入模式
GPIO_InitStruct.Pull = GPIO_NOPULL; // 不能有上下拉可能会有影响
HAL_GPIO_Init(ADC_CUR_GPIO, &GPIO_InitStruct);
}
}
DMA与ADC结构体中每个变量的含义自己追代码即可。
main.c
#include "stm32f4xx_hal.h"
#include "key/bsp_key.h"
#include "encoder/bsp_encoder.h"
#include "usart/bsp_usartx.h"
#include "adc/bsp_adc.h"
#include "DCMotor/bsp_BDCMotor.h"
#include "stdlib.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 ADC_Base 8 // 取2的整数倍作为缓存区大小,得到14bits
/* 使用DMA传输数据,采集n个数据点的时间是0.65ms,采样率大约是 1500 KHz */
#define ADC_BUFFER 1024 // 采样数据缓存区
/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
#define CUR_P_DATA 0.35f // P参数
#define CUR_I_DATA 0.6f // I参数
#define CUR_D_DATA 0.0f // D参数
#define TARGET_CURRENT 200 // 最大电流值 200mA
#define SPD_P_DATA 1.5f // P参数
#define SPD_I_DATA 0.5f // I参数
#define SPD_D_DATA 0.0f // D参数
#define TARGET_SPEED 10.0f // 目标速度 10r/m
#define ENCODER 11 // 编码器线数
#define SPEEDRATIO 30 // 电机减速比
#define PPR (SPEEDRATIO*ENCODER*4)
/* 私有变量 ------------------------------------------------------------------*/
__IO uint8_t Start_flag = 0; // PID 开始标志
uint32_t Motor_Dir = CW; // 电机方向
__IO int32_t tmpPWM_Duty = 0;
/* 用于保存转换计算后的数值 */
__IO float ADC_VoltBus; // 总线电压值
__IO int32_t Spd_Pulse; // 编码器捕获值 Pulse
__IO int32_t LastSpd_Pulse; // 编码器捕获值 Pulse
__IO int32_t Spd_PPS; // 速度值 Pulse/Sample
__IO float Spd_RPM; // 速度值 r/m
/* AD转换结果值 */
__IO int16_t ADC_ConvValueHex[ADC_BUFFER]; // AD转换结果缓存
__IO int32_t AverSum = 0; // 平均值的累加值
__IO int32_t AverCnt = 0; // 平均值的计数器
__IO uint32_t OffsetCnt_Flag = 0 ; // 偏差值的计数器标志
__IO int32_t OffSetHex ; // 偏差值
/* 扩展变量 ------------------------------------------------------------------ */
extern __IO uint32_t uwTick;
/* PID结构体 */
PID_TypeDef cPID,sPID; // PID参数结构体
/* 扩展变量 ------------------------------------------------------------------*/
/* 私有函数原形 --------------------------------------------------------------*/
void PID_ParamInit(void) ;
int32_t CurPIDCalc(int32_t NextPoint);
int32_t SpdPIDCalc(float NextPoint);
int32_t ADC_GetSampleAvgN(int16_t *Data, uint32_t N);
/* 函数体 --------------------------------------------------------------------*/
/**
* 函数功能: 系统时钟配置
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
__HAL_RCC_PWR_CLK_ENABLE(); // 使能PWR时钟
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); // 设置调压器输出电压级别1
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // 外部晶振,8MHz
RCC_OscInitStruct.HSEState = RCC_HSE_ON; // 打开HSE
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; // 打开PLL
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; // PLL时钟源选择HSE
RCC_OscInitStruct.PLL.PLLM = 8; // 8分频MHz
RCC_OscInitStruct.PLL.PLLN = 336; // 336倍频
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // 2分频,得到168MHz主时钟
RCC_OscInitStruct.PLL.PLLQ = 7; // USB/SDIO/随机数产生器等的主PLL分频系数
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; // 系统时钟:168MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // AHB时钟: 168MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // APB1时钟:42MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // APB2时钟:84MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
HAL_RCC_EnableCSS(); // 使能CSS功能,优先使用外部晶振,内部时钟源为备用
// 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();
/* 串口初始化 */
MX_USARTx_Init();
/* 按键初始化 */
KEY_GPIO_Init();
/* 编码器初始化及使能编码器模式 */
ENCODER_TIMx_Init();
/* ADC-DMA 初始化 */
MX_ADCx_Init();
/* 启动AD转换并使能DMA传输和中断 */
HAL_ADC_Start_DMA(&hadcx,(uint32_t*)ADC_ConvValueHex,ADC_BUFFER);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_HT);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_TE);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_FE);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_DME);
/* 高级控制定时器初始化并配置PWM输出功能 */
BDCMOTOR_TIMx_Init();
/* 启动定时器通道和互补通道PWM输出 */
PWM_Duty = 0;
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty); // 0%
/* PID 参数初始化 */
PID_ParamInit();
/* 无限循环 */
while (1)
{
/* 停止按钮启动PWM Start_flag = 1 但占空比为0 */
if(KEY1_StateRead()==KEY_DOWN)
{
HAL_TIM_PWM_Start(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,0);
Start_flag = 1;
}
if(KEY2_StateRead()==KEY_DOWN)
{
SHUTDOWN_MOTOR();
HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1); // 停止输出
}
if(KEY3_StateRead()==KEY_DOWN)//加速
{
sPID.SetPoint += 2;
if(sPID.SetPoint >= 42)
sPID.SetPoint = 42;
}
if(KEY4_StateRead()==KEY_DOWN)//减速
{
sPID.SetPoint -= 2;
if(sPID.SetPoint <=-42)
sPID.SetPoint = -42;
}
}
}
/**
* 函数功能: 系统滴答定时器中断回调函数
* 输入参数: 无
* 返 回 值: 无
* 说 明: 每发生一次滴答定时器中断进入该回调函数一次
*/
void HAL_SYSTICK_Callback(void)
{
__IO int32_t ADC_Resul= 0;
__IO float Volt_Result = 0;
__IO float ADC_CurrentValue; // 电流
/* 速度环周期100ms */
if(uwTick % 100 == 0)
{
Spd_Pulse = (OverflowCount*CNT_MAX) + (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
Spd_PPS = Spd_Pulse - LastSpd_Pulse;
LastSpd_Pulse = Spd_Pulse ;
/* 11线编码器,30减速比,一圈脉冲信号是11*30*4 = 1320 */
Spd_RPM = ((((float)Spd_PPS/(float)PPR)*10.0f)*(float)60); // 10的含义:100ms *10 化成秒 60的含义:转每分钟
/* 计算PID结果 */
if(Start_flag == 1)
{
tmpPWM_Duty = SpdPIDCalc(Spd_RPM);
/* 根据速度环的计算结果判断当前运动方向 */
if(tmpPWM_Duty < 0)
{
Motor_Dir = CW;
BDDCMOTOR_DIR_CW();
tmpPWM_Duty = -tmpPWM_Duty;
}
else
{
Motor_Dir = CCW;
BDDCMOTOR_DIR_CCW();
}
/* 设定电流环的目标值,电流没有负数 */
if(tmpPWM_Duty >= TARGET_CURRENT)
tmpPWM_Duty = TARGET_CURRENT;
}
}
/* 电流环周期是40ms,电流单次采集周期大约是 2ms,最好不要低于2ms */
if(uwTick % 40 == 0)
{
ADC_Resul = AverSum/AverCnt ;
/* 连续采样16次以后,作为偏差值 */
OffsetCnt_Flag++;
if(OffsetCnt_Flag >= 16)
{
if(OffsetCnt_Flag == 16)
{
OffSetHex /= OffsetCnt_Flag-1;
}
OffsetCnt_Flag = 32;
ADC_Resul -= OffSetHex;//减去偏差值
}
else
OffSetHex += ADC_Resul;
/* 计算电压值和电流值 */
Volt_Result = ( (float)( (float)(ADC_Resul) * VOLT_RESOLUTION) );
ADC_CurrentValue = (float)( (Volt_Result / GAIN) / SAMPLING_RES);
// 驱动板功耗大约是10~17mA,这里是为了方便观察电流精度才+10.
ADC_CurrentValue += 10;
if(Volt_Result<0)
Volt_Result = 0;
/* 清空计数 */
AverCnt = 0;
AverSum = 0;
/* 计算PID结果 */
if(Start_flag == 1)
{
cPID.SetPoint = tmpPWM_Duty ;
PWM_Duty = CurPIDCalc( (int32_t)ADC_CurrentValue);
if(PWM_Duty >= BDCMOTOR_DUTY_FULL)
PWM_Duty = BDCMOTOR_DUTY_FULL;
if(PWM_Duty <=0)
PWM_Duty = 0;
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty);
}
printf("Sped: %-2.2f r/m Curr: %d mA \n",Spd_RPM ,(int32_t)ADC_CurrentValue); // 参数1:转每分钟 参数2:电流
}
}
/**
* 函数功能: ADC转换完成回调函数
* 输入参数: hadc:ADC外设设备句柄
* 返 回 值: 无
* 说 明: 中断一次的时间是1.479ms,利用过采样和求均值方法,提高分辨率
*/
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
int32_t ADConv = 0 ;
/* ADC采集太快,需要先停止再处理数据 */
HAL_ADC_Stop_DMA(hadc);
/* 取平均 */
ADConv = ADC_GetSampleAvgN((int16_t*)&ADC_ConvValueHex,ADC_BUFFER);
/* 累加采样结果并记录采样次数*/
AverSum += ADConv;
AverCnt++;
HAL_ADC_Start_DMA(hadc,(uint32_t*)ADC_ConvValueHex,ADC_BUFFER);
}
/**
* 函数功能: 得到N 个ADC 采样的均值
* 输入参数: 要做平均的ADC 采样数
* 返 回 值: 均值
* 说 明: 计算平均值,获得14bitsADC值
*/
int32_t ADC_GetSampleAvgN(int16_t *Data, uint32_t N)
{
int32_t avg_sample =0x00;
uint32_t index=0x00;
/* 累加N 个ADC 采样 */
for (index =0; index < N; index++)
{
avg_sample += ((int32_t)Data[index]);
}
/* 计算N个ADC 采样的均值 */
avg_sample >>= ADC_Base;
/* 返回均值 */
return avg_sample;
}
/******************** PID 控制设计 ***************************/
/**
* 函数功能: PID参数初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void PID_ParamInit()
{
cPID.LastError = 0; // Error[-1]
cPID.SumError= 0; // 误差累积
cPID.Proportion = CUR_P_DATA; // 比例常数 Proportional Const
cPID.Integral = CUR_I_DATA; // 积分常数 Integral Const
cPID.Derivative = CUR_D_DATA; // 微分常数 Derivative Const
cPID.SetPoint = TARGET_CURRENT;// 设定目标Desired Value
sPID.LastError = 0; // Error[-1]
cPID.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
}
/**
* 函数名称:电流闭环PID控制设计
* 输入参数:当前控制量
* 返 回 值:目标控制量
* 说 明:无
*/
int32_t CurPIDCalc(int32_t NextPoint)
{
int32_t iError,dError;
iError = cPID.SetPoint - NextPoint; //偏差
/* 设定闭环死区 */
if((iError >= -3) && (iError <= 3))
iError = 0;
cPID.SumError += iError; //积分
dError = iError - cPID.LastError; //微分
cPID.LastError = iError;
return (int32_t)(cPID.Proportion * (float)iError //比例项
+ cPID.Integral * (float)cPID.SumError //积分项
+ cPID.Derivative * (float)dError); //微分项
}
/**
* 函数名称:速度闭环PID控制设计
* 输入参数:当前控制量
* 返 回 值:目标控制量
* 说 明:无
*/
int32_t SpdPIDCalc(float NextPoint)
{
float iError,dError;
iError = sPID.SetPoint - NextPoint; //偏差
if((iError<0.3f )&& (iError>-0.3f))
iError = 0.0f;
sPID.SumError += iError; //积分
/* 设定积分上限 */
if(sPID.SumError >= (TARGET_CURRENT*10.0f))
sPID.SumError = (TARGET_CURRENT*10.0f);
if(sPID.SumError <= -(TARGET_CURRENT*10.0f))
sPID.SumError = -(TARGET_CURRENT*10.0f);
dError = iError - sPID.LastError; //微分
sPID.LastError = iError;
return (int32_t)(sPID.Proportion * iError //比例项
+ sPID.Integral * (float)sPID.SumError //积分项
+ sPID.Derivative * dError); //微分项
}
实验结果:
3.位置速度电流(三闭环)(三个PID都是位置式PID)
ADC,编码器,高级定时器PWM跟上面两个列子一样。只是main.c不同
main.c
/* 包含头文件 ----------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "key/bsp_key.h"
#include "encoder/bsp_encoder.h"
#include "usart/bsp_usartx.h"
#include "adc/bsp_adc.h"
#include "DCMotor/bsp_BDCMotor.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]
}PID_TypeDef;
/* 私有宏定义 ----------------------------------------------------------------*/
#define ADC_Base 8 // 取2的整数倍作为缓存区大小,得到14bits
/* 使用DMA传输数据,采集n个数据点的时间是0.65ms,采样率大约是 1500 KHz */
#define ADC_BUFFER 1024 // 采样数据缓存区
#define SPEEDRATIO 30
#define ENCODER_RESOLUTION 11
#define PPR ((SPEEDRATIO*ENCODER_RESOLUTION)*4) // Pulse/Round 每圈可捕获到的脉冲数
/*************************************/
// 定义PID相关宏
// 这三个参数设定对电机运行影响非常大
// PID参数跟采样时间息息相关
/*************************************/
#define CUR_P_DATA 0.35f // P参数
#define CUR_I_DATA 0.6f // I参数
#define CUR_D_DATA 0.0f // D参数
#define TARGET_CURRENT 50 // 最大电流值 100mA
#define SPD_P_DATA 4.5f // P参数
#define SPD_I_DATA 0.5f // I参数
#define SPD_D_DATA 0.0f // D参数
#define TARGET_SPEED 20.0f // 目标速度 20r/m
#define LOC_P_DATA 0.009f // P参数
#define LOC_I_DATA 0.002f // I参数
#define LOC_D_DATA 0.04f // D参数
#define TARGET_LOC (10*PPR) // 目标位置 10圈
/* 私有变量 ------------------------------------------------------------------*/
__IO uint8_t Start_flag = 0; // PID 开始标志
uint32_t Motor_Dir = CW; // 电机方向
__IO int32_t tmpPWM_DutySpd = 0;
__IO int32_t tmpPWM_Duty = 0;
/* 用于保存转换计算后的数值 */
__IO float ADC_VoltBus; // 总线电压值
__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
/* AD转换结果值 */
__IO int16_t ADC_ConvValueHex[ADC_BUFFER]; // AD转换结果缓存
__IO int32_t AverSum = 0; // 平均值的累加值
__IO int32_t AverCnt = 0; // 平均值的计数器
__IO uint32_t OffsetCnt_Flag = 0 ; // 偏差值的计数器标志
__IO int32_t OffSetHex ; // 偏差值
/* 扩展变量 ------------------------------------------------------------------ */
extern __IO uint32_t uwTick;
/* PID结构体 */
PID_TypeDef cPID,sPID,lPID; // PID参数结构体
/* 扩展变量 ------------------------------------------------------------------*/
/* 私有函数原形 --------------------------------------------------------------*/
void PID_ParamInit(void) ;
int32_t CurPIDCalc(int32_t NextPoint);
int32_t SpdPIDCalc(float NextPoint);
int32_t LocPIDCalc(int32_t NextPoint);
int32_t ADC_GetSampleAvgN(int16_t *Data, uint32_t N );
/* 函数体 --------------------------------------------------------------------*/
/**
* 函数功能: 系统时钟配置
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct;
RCC_ClkInitTypeDef RCC_ClkInitStruct;
__HAL_RCC_PWR_CLK_ENABLE(); // 使能PWR时钟
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); // 设置调压器输出电压级别1
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; // 外部晶振,8MHz
RCC_OscInitStruct.HSEState = RCC_HSE_ON; // 打开HSE
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; // 打开PLL
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; // PLL时钟源选择HSE
RCC_OscInitStruct.PLL.PLLM = 8; // 8分频MHz
RCC_OscInitStruct.PLL.PLLN = 336; // 336倍频
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // 2分频,得到168MHz主时钟
RCC_OscInitStruct.PLL.PLLQ = 7; // USB/SDIO/随机数产生器等的主PLL分频系数
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; // 系统时钟:168MHz
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // AHB时钟: 168MHz
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // APB1时钟:42MHz
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // APB2时钟:84MHz
HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5);
HAL_RCC_EnableCSS(); // 使能CSS功能,优先使用外部晶振,内部时钟源为备用
// 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();
/* 串口初始化 */
MX_USARTx_Init();
/* 按键初始化 */
KEY_GPIO_Init();
/* 编码器初始化及使能编码器模式 */
ENCODER_TIMx_Init();
/* ADC-DMA 初始化 */
MX_ADCx_Init();
/* 启动AD转换并使能DMA传输和中断 */
HAL_ADC_Start_DMA(&hadcx,(uint32_t*)ADC_ConvValueHex,ADC_BUFFER);
// 失能DMA的一些标志
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_HT);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_TE);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_FE);
__HAL_DMA_DISABLE_IT(&hdma_adcx,DMA_IT_DME);
/* 高级控制定时器初始化并配置PWM输出功能 */
BDCMOTOR_TIMx_Init();
/* 启动定时器通道和互补通道PWM输出 */
PWM_Duty = 0;
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty); // 0%
/* PID 参数初始化 */
PID_ParamInit();
/* 无限循环 */
while (1)
{
/* 停止按钮 先按下KEY1启动PWM */
if(KEY1_StateRead()==KEY_DOWN)
{
HAL_TIM_PWM_Start(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,0);
Start_flag = 1;
}
if(KEY2_StateRead()==KEY_DOWN)
{
SHUTDOWN_MOTOR();
HAL_TIM_PWM_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1);
HAL_TIMEx_PWMN_Stop(&htimx_BDCMOTOR,TIM_CHANNEL_1); // 停止输出
}
if(KEY3_StateRead()==KEY_DOWN) // 圈数+1
{
lPID.SetPoint += PPR;
}
if(KEY4_StateRead()==KEY_DOWN) // 圈数-1
{
lPID.SetPoint -= PPR;
}
}
}
/**
* 函数功能: 系统滴答定时器中断回调函数
* 输入参数: 无
* 返 回 值: 无
* 说 明: 每隔一定的时间就执行pid算法
*/
void HAL_SYSTICK_Callback(void)
{
__IO int32_t ADC_Resul= 0;
__IO float Volt_Result = 0;
__IO float ADC_CurrentValue; // 电流
/* 位置环周期250ms */
if(uwTick % 250 == 0)
{
/* 获取当前位置值,编码器4倍频之后的数值 (OverflowCount*CNT_MAX) +*/
Sample_Pulse = (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;
}
}
/* 速度环周期100ms */
if(uwTick % 100 == 0)
{
/* 获得当前速度 (OverflowCount*CNT_MAX) +*/
Sample_Pulse = (int32_t)__HAL_TIM_GET_COUNTER(&htimx_Encoder);
Spd_PPS = Sample_Pulse - LastSample_Pulse;
LastSample_Pulse = Sample_Pulse ;
/* 11线编码器,30减速比,一圈脉冲信号是11*30*4 PPR */
Spd_RPM = ((((float)Spd_PPS/(float)PPR)*10.0f)*(float)60);//单位是rpm
/* 计算PID结果 */
if(Start_flag == 1)
{
sPID.SetPoint = tmpPWM_DutySpd;
tmpPWM_Duty = SpdPIDCalc(Spd_RPM);
/* 根据速度环的计算结果判断当前运动方向 */
if(tmpPWM_Duty < 0)
{
Motor_Dir = CW;
BDDCMOTOR_DIR_CW();
tmpPWM_Duty = -tmpPWM_Duty;
}
else
{
Motor_Dir = CCW;
BDDCMOTOR_DIR_CCW();
}
/* 设定电流环的目标值,电流没有负数 */
if(tmpPWM_Duty >= TARGET_CURRENT)
tmpPWM_Duty = TARGET_CURRENT;
}
}
/* 电流环周期是40ms,电流单次采集周期大约是 2ms,最好不要低于2ms */
if(uwTick % 40 == 0)
{
ADC_Resul = AverSum/AverCnt ;
/* 连续采样16次以后,作为偏差值 */
OffsetCnt_Flag++;
if(OffsetCnt_Flag >= 16)
{
if(OffsetCnt_Flag == 16)
{
OffSetHex /= OffsetCnt_Flag-1;
}
OffsetCnt_Flag = 32;
ADC_Resul -= OffSetHex;//减去偏差值
}
else
OffSetHex += ADC_Resul;
/* 计算电压值和电流值 */
Volt_Result = ( (float)( (float)(ADC_Resul) * VOLT_RESOLUTION) );
ADC_CurrentValue = (float)( (Volt_Result / GAIN) / SAMPLING_RES);
if(Volt_Result<0)
Volt_Result = 0;
/* 清空计数 */
AverCnt = 0;
AverSum = 0;
/* 计算PID结果 */
if(Start_flag == 1)
{
cPID.SetPoint = tmpPWM_Duty ;
PWM_Duty = CurPIDCalc( (int32_t)ADC_CurrentValue);
if(PWM_Duty >= BDCMOTOR_DUTY_FULL)
PWM_Duty = BDCMOTOR_DUTY_FULL;
if(PWM_Duty <=0)
PWM_Duty = 0;
__HAL_TIM_SET_COMPARE(&htimx_BDCMOTOR,TIM_CHANNEL_1,PWM_Duty);
}
printf("LOC:%d Sped: %2.2f r/m Curr: %d mA \n",Sample_Pulse,
Spd_RPM ,(int32_t)ADC_CurrentValue);
// 参数1:位置目标的总的脉冲数 参数2: 转每分钟 参数3:电流
}
}
/**
* 函数功能: ADC转换完成回调函数
* 输入参数: hadc:ADC外设设备句柄
* 返 回 值: 无
* 说 明: 中断一次的时间是1.479ms,利用过采样和求均值方法,提高分辨率
*/
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
int32_t ADConv = 0 ;
/* ADC采集太快,需要先停止再处理数据 */
HAL_ADC_Stop_DMA(hadc);
/* 去掉高和低总共SORT_NUM个采样数据,取中间部分的数据做平均 */
ADConv = ADC_GetSampleAvgN((int16_t*)&ADC_ConvValueHex,ADC_BUFFER);
/* 累加采样结果并记录采样次数*/
AverSum += ADConv;
AverCnt++;
HAL_ADC_Start_DMA(hadc,(uint32_t*)ADC_ConvValueHex,ADC_BUFFER);
}
/**
* 函数功能: 得到N 个ADC 采样的均值
* 输入参数: 要做平均的ADC 采样数
* 返 回 值: 均值
* 说 明: 计算平均值,获得14bitsADC值
*/
int32_t ADC_GetSampleAvgN(int16_t *Data, uint32_t N)
{
int32_t avg_sample =0x00;
uint32_t index=0x00;
/* 累加N 个ADC 采样 */
for (index = 0; index < N; index++)
{
avg_sample += ((int32_t)Data[index]);
}
/* 计算N 个ADC 采样的均值 */
avg_sample >>= ADC_Base;
/* 返回均值 */
return avg_sample;
}
/******************** PID 控制设计 ***************************/
/**
* 函数功能: PID参数初始化
* 输入参数: 无
* 返 回 值: 无
* 说 明: 无
*/
void PID_ParamInit()
{
cPID.LastError = 0; // Error[-1]
cPID.SumError= 0; // 误差累积
cPID.Proportion = CUR_P_DATA; // 比例常数 Proportional Const
cPID.Integral = CUR_I_DATA; // 积分常数 Integral Const
cPID.Derivative = CUR_D_DATA; // 微分常数 Derivative Const
cPID.SetPoint = TARGET_CURRENT;// 设定目标Desired Value
sPID.LastError = 0; // Error[-1]
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.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控制设计
* 输入参数:当前控制量
* 返 回 值:目标控制量
* 说 明:无
*/
int32_t CurPIDCalc(int32_t NextPoint)
{
int32_t iError,dError;
iError = cPID.SetPoint - NextPoint; //偏差
/* 设定闭环死区 */
if((iError >= -3) && (iError <= 3))
iError = 0;
cPID.SumError += iError; //积分
dError = iError - cPID.LastError; //微分
cPID.LastError = iError;
return (int32_t)(cPID.Proportion * (float)iError //比例项
+ cPID.Integral * (float)cPID.SumError //积分项
+ cPID.Derivative * (float)dError); //微分项
}
/**
* 函数名称:速度闭环PID控制设计
* 输入参数:当前控制量
* 返 回 值:目标控制量
* 说 明:无
*/
int32_t SpdPIDCalc(float NextPoint)
{
float iError,dError;
iError = sPID.SetPoint - NextPoint; //偏差
if((iError<0.3f )&& (iError>-0.3f))
iError = 0.0f;
sPID.SumError += iError; //积分
/* 设定积分上限 */
if(sPID.SumError >= (TARGET_CURRENT*10.0f))
sPID.SumError = (TARGET_CURRENT*10.0f);
if(sPID.SumError <= -(TARGET_CURRENT*10.0f))
sPID.SumError = -(TARGET_CURRENT*10.0f);
dError = iError - sPID.LastError; //微分
sPID.LastError = iError;
return (int32_t)(sPID.Proportion * iError //比例项
+ sPID.Integral * (float)sPID.SumError //积分项
+ sPID.Derivative * 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 = 0;
}
/* 积分分离 */
if((iError >= -200) && (iError <= 200))
{
lPID.SumError += iError; //积分
/* 设定积分上限 */
if(lPID.SumError >= 2000)
lPID.SumError = 2000;
if(lPID.SumError <= -2000)
lPID.SumError = -2000;
}
dError = iError - lPID.LastError; //微分
lPID.LastError = iError;
return (int32_t)(lPID.Proportion * (float)iError //比例项
+ lPID.Integral * (float)lPID.SumError //积分项
+ lPID.Derivative * (float)dError); //微分项
}
有三个PID结构体一定记住:外环的输出做内环的目标值,内环的输出去控制被控制量。
实验结果:
4.总结
多闭环中每一环的PID参数都会影响,所以PID参数调节是很重要的.需要不断的多调才能有经验。不管位置,速度,电流它们的PID算法都是一样。如果是其它参数也可以用PID算法做闭环。