电机应用-直流有刷电机多环控制实现

目录

直流有刷电机多环控制实现

硬件设计

直流电机三环(速度环、电流环、位置环)串级PID控制-位置式PID

编程要点

配置ADC可读取电流值

配置基本定时器6产生定时中断读取当前电路中驱动电机的电流值并执行PID运算

配置定时器1输出PWM控制电机

配置定时器3读取编码器的计数值

ADC数据处理

编写位置式PID算法


直流有刷电机多环控制实现

外环的输出会作为内环的输入。外环一般是最终要控制的效果, 

电机应用-直流有刷电机多环控制实现_第1张图片

硬件设计

可选:L298N电机驱动板、野火MOS搭建的驱动板。

电机应用-直流有刷电机多环控制实现_第2张图片

电机应用-直流有刷电机多环控制实现_第3张图片

直流电机三环(速度环、电流环、位置环)串级PID控制-位置式PID

编程要点

配置ADC可读取电流值。

配置基本定时器TIM6产生定时中断执行PID运算。

配置高级定时器TIM1输出PWM控制电机。

配置通用定时器TIM3读取编码器的计数值。

ADC数据处理。

编写位置式PID算法。

编写位置环、速度环、电流环控制函数。

增加上位机曲线观察相关代码。

编写按键控制代码。

配置ADC可读取电流值
#define VBUS_MAX      		14    // 电压最大值
#define VBUS_MIN      		10    // 电压最小值
			
#define VBUS_HEX_MAX  		((VBUS_MAX/37.0+1.24)/VREF*65536)    // 电压最大值(测量电压是电源电压的1/37)
#define VBUS_HEX_MIN  		((VBUS_MIN/37.0+1.24)/VREF*65536)    // 电压最小值(测量电压是电源电压的1/37)
 
__IO uint16_t ADC_ConvertedValue;
DMA_HandleTypeDef DMA_Init_Handle;
ADC_HandleTypeDef ADC_Handle;
 
static uint16_t adc_buff[1024];
static uint16_t vbus_adc_mean = 0;  	// 电源电压 ADC 采样结果平均值
static uint32_t adc_mean_sum = 0;   	// 平均值累加
static uint32_t adc_mean_count = 0; 	// 累加计数
 
/**
  * @brief  电流采集初始化
  * @param  无
  * @retval 无
  */
void ADC_Init(void)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	
    __GPIOB_CLK_ENABLE();
	__DMA2_CLK_ENABLE();
	__ADC1_CLK_ENABLE();
 
	// PB1--电流
    GPIO_InitStructure.Pin = GPIO_PIN_1;
    GPIO_InitStructure.Mode = GPIO_MODE_ANALOG;
    GPIO_InitStructure.Pull = GPIO_NOPULL;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
 
	// PB0--电压
    GPIO_InitStructure.Pin = GPIO_PIN_0;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStructure);
	
	// ADC1使用DMA2,数据流0,通道0,这个是手册固定死的
	DMA_Init_Handle.Instance 					= DMA2_Stream0;
    DMA_Init_Handle.Init.Direction 				= DMA_PERIPH_TO_MEMORY;
    DMA_Init_Handle.Init.PeriphInc 				= DMA_PINC_DISABLE;
    DMA_Init_Handle.Init.MemInc 				= DMA_MINC_ENABLE;
    DMA_Init_Handle.Init.PeriphDataAlignment 	= DMA_PDATAALIGN_HALFWORD;
    DMA_Init_Handle.Init.MemDataAlignment 		= DMA_MDATAALIGN_HALFWORD;
    DMA_Init_Handle.Init.Mode 					= DMA_CIRCULAR;
    DMA_Init_Handle.Init.Priority 				= DMA_PRIORITY_HIGH;
    DMA_Init_Handle.Init.FIFOMode 				= DMA_FIFOMODE_DISABLE;
    DMA_Init_Handle.Init.FIFOThreshold 			= DMA_FIFO_THRESHOLD_HALFFULL;
    DMA_Init_Handle.Init.MemBurst 				= DMA_MBURST_SINGLE;
    DMA_Init_Handle.Init.PeriphBurst 			= DMA_PBURST_SINGLE;
    // 选择 DMA 通道,通道存在于流中
    DMA_Init_Handle.Init.Channel 				= DMA_CHANNEL_0;
    //初始化DMA流,流相当于一个大的管道,管道里面有很多通道
    HAL_DMA_Init(&DMA_Init_Handle);
 
    __HAL_LINKDMA(&ADC_Handle, DMA_Handle, DMA_Init_Handle);
	
	ADC_Handle.Instance 					= ADC1;
    ADC_Handle.Init.ClockPrescaler 			= ADC_CLOCKPRESCALER_PCLK_DIV4;
    ADC_Handle.Init.Resolution 				= ADC_RESOLUTION_12B;
    ADC_Handle.Init.ScanConvMode 			= ENABLE;
    ADC_Handle.Init.ContinuousConvMode 		= ENABLE;
    ADC_Handle.Init.DiscontinuousConvMode 	= DISABLE;
    ADC_Handle.Init.NbrOfDiscConversion   	= 0;
    ADC_Handle.Init.ExternalTrigConvEdge 	= ADC_EXTERNALTRIGCONVEDGE_NONE;
    ADC_Handle.Init.ExternalTrigConv 		= ADC_SOFTWARE_START;
    ADC_Handle.Init.DataAlign 				= ADC_DATAALIGN_LEFT;
    ADC_Handle.Init.NbrOfConversion 		= 2;
    ADC_Handle.Init.DMAContinuousRequests 	= ENABLE;
    ADC_Handle.Init.EOCSelection          	= ADC_EOC_SINGLE_CONV;
    HAL_ADC_Init(&ADC_Handle);
	
	ADC_ChannelConfTypeDef ADC_Config;
 
    ADC_Config.Channel      = ADC_CHANNEL_9;
    ADC_Config.Rank         = 1;
    ADC_Config.SamplingTime = ADC_SAMPLETIME_3CYCLES;
    ADC_Config.Offset       = 0;
    HAL_ADC_ConfigChannel(&ADC_Handle, &ADC_Config);
 
    /** Configure the analog watchdog
    */
    ADC_AnalogWDGConfTypeDef AnalogWDGConfig = {0};
 
    AnalogWDGConfig.WatchdogMode 	= ADC_ANALOGWATCHDOG_SINGLE_REG;
    AnalogWDGConfig.HighThreshold 	= VBUS_HEX_MAX;
    AnalogWDGConfig.LowThreshold 	= VBUS_HEX_MIN;
    AnalogWDGConfig.Channel 		= ADC_CHANNEL_8;
    AnalogWDGConfig.ITMode 			= ENABLE;
 
    if (HAL_ADC_AnalogWDGConfig(&ADC_Handle, &AnalogWDGConfig) != HAL_OK)
    {
        while (1);
    }
 
    /** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
    */
    ADC_Config.Channel 		= ADC_CHANNEL_8;
    ADC_Config.Rank 		= 2;
    ADC_Config.SamplingTime = ADC_SAMPLETIME_3CYCLES;
    ADC_Config.Offset       = 0;
 
    if (HAL_ADC_ConfigChannel(&ADC_Handle, &ADC_Config) != HAL_OK)
    {
        while (1);
    }
 
    // 外设中断优先级配置和使能中断配置
    HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 4, 0);
    HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
 
    HAL_NVIC_SetPriority(ADC_IRQn, 3, 0);
    HAL_NVIC_EnableIRQ(ADC_IRQn);
 
    HAL_ADC_Start_DMA(&ADC_Handle, (uint32_t *)&adc_buff, 1024);
}

配置基本定时器6产生定时中断读取当前电路中驱动电机的电流值并执行PID运算
TIM_HandleTypeDef TIM_TimeBaseStructure;
 
/**
  * @brief  初始化基本定时器定时,默认50ms产生一次中断
  * @param  无
  * @retval 无
  */
void TIMx_Configuration(void)
{
    HAL_NVIC_SetPriority(TIM6_DAC_IRQn, 1, 3);
    HAL_NVIC_EnableIRQ(TIM6_DAC_IRQn);
	
	__TIM6_CLK_ENABLE();
	
	TIM_TimeBaseStructure.Instance = TIM6;
    TIM_TimeBaseStructure.Init.Period = 50 * 50 - 1;
    TIM_TimeBaseStructure.Init.Prescaler = 1680 - 1;
    TIM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
    TIM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    HAL_TIM_Base_Init(&TIM_TimeBaseStructure);
 
    // 开启定时器更新中断
    HAL_TIM_Base_Start_IT(&TIM_TimeBaseStructure);
	
    uint32_t temp = (__HAL_TIM_GET_AUTORELOAD(&TIM_TimeBaseStructure) + 1) / 50.0;  // 计算周期,单位ms
    set_computer_value(SEND_PERIOD_CMD, CURVES_CH1, &temp, 1);  					// 给通道 1 发送目标值
}
 
/**
  * @brief  定时器更新事件回调函数
  * @param  无
  * @retval 无
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == (&TIM_TimeBaseStructure))
    {
        motor_pid_control();	// 每50ms执行一次PID运算
    }
}

配置定时器1输出PWM控制电机
TIM_HandleTypeDef  DCM_TimeBaseStructure;
 
/**
  * @brief  初始化控制通用定时器
  * @param  无
  * @retval 无
  */
void Motor_TIMx_Configuration(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OC_InitTypeDef  TIM_OCInitStructure;
 
	__HAL_RCC_GPIOA_CLK_ENABLE();
    __TIM1_CLK_ENABLE();
 
	// PA8--PWM_TIM_CH1
	GPIO_InitStruct.Pin 		= GPIO_PIN_8;
    GPIO_InitStruct.Mode 		= GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed 		= GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate 	= PWM_TIM_GPIO_AF;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
	// PA9--PWM_TIM_CH2
    GPIO_InitStruct.Pin 		= GPIO_PIN_9;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
	// TIM1 66.7us一次周期
    DCM_TimeBaseStructure.Instance = TIM1;
    DCM_TimeBaseStructure.Init.Period = 5600 - 1;
    DCM_TimeBaseStructure.Init.Prescaler = 1 - 1;
    DCM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
    DCM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    HAL_TIM_PWM_Init(&DCM_TimeBaseStructure);
 
    /*PWM模式配置*/
    TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
    TIM_OCInitStructure.Pulse = 0;
    TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
    TIM_OCInitStructure.OCNPolarity = TIM_OCNPOLARITY_HIGH;
    TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
    TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
 
    /*配置PWM通道*/
    HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_1);
    /*开始输出PWM*/
    HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_1);
 
    /*配置PWM通道*/
    HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_2);
    /*开始输出PWM*/
    HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_2);
}
 
/**
  * @brief  设置TIM通道的占空比
	* @param  channel		通道	(1,2,3,4)
	* @param  compare		占空比
	*	@note 	无
  * @retval 无
  */
void TIM1_SetPWM_pulse(uint32_t channel, int compare)
{
    switch (channel)
    {
        case TIM_CHANNEL_1:
            __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_1, compare);
            break;
 
        case TIM_CHANNEL_2:
            __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_2, compare);
            break;
 
        case TIM_CHANNEL_3:
            __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_3, compare);
            break;
 
        case TIM_CHANNEL_4:
            __HAL_TIM_SET_COMPARE(&DCM_TimeBaseStructure, TIM_CHANNEL_4, compare);
            break;
    }
}

配置定时器3读取编码器的计数值
TIM_HandleTypeDef  DCM_TimeBaseStructure;
 
/**
  * @brief  初始化控制通用定时器
  * @param  无
  * @retval 无
  */
void Motor_TIMx_Configuration(void)
{
    GPIO_InitTypeDef GPIO_InitStruct;
    TIM_OC_InitTypeDef  TIM_OCInitStructure;
 
	__HAL_RCC_GPIOA_CLK_ENABLE();
    __TIM1_CLK_ENABLE();
 
	// PA8--PWM_TIM_CH1
	GPIO_InitStruct.Pin 		= GPIO_PIN_8;
    GPIO_InitStruct.Mode 		= GPIO_MODE_AF_PP;
    GPIO_InitStruct.Speed 		= GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.Alternate 	= PWM_TIM_GPIO_AF;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
	// PA9--PWM_TIM_CH2
    GPIO_InitStruct.Pin 		= GPIO_PIN_9;
    HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
	// TIM1 66.7us一次周期
    DCM_TimeBaseStructure.Instance = TIM1;
    DCM_TimeBaseStructure.Init.Period = 5600 - 1;
    DCM_TimeBaseStructure.Init.Prescaler = 1 - 1;
    DCM_TimeBaseStructure.Init.CounterMode = TIM_COUNTERMODE_UP;
    DCM_TimeBaseStructure.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    HAL_TIM_PWM_Init(&DCM_TimeBaseStructure);
 
    /*PWM模式配置*/
    TIM_OCInitStructure.OCMode = TIM_OCMODE_PWM1;
    TIM_OCInitStructure.Pulse = 0;
    TIM_OCInitStructure.OCPolarity = TIM_OCPOLARITY_HIGH;
    TIM_OCInitStructure.OCNPolarity = TIM_OCNPOLARITY_HIGH;
    TIM_OCInitStructure.OCIdleState = TIM_OCIDLESTATE_SET;
    TIM_OCInitStructure.OCNIdleState = TIM_OCNIDLESTATE_RESET;
 
    /*配置PWM通道*/
    HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_1);
    /*开始输出PWM*/
    HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_1);
 
    /*配置PWM通道*/
    HAL_TIM_PWM_ConfigChannel(&DCM_TimeBaseStructure, &TIM_OCInitStructure, TIM_CHANNEL_2);
    /*开始输出PWM*/
    HAL_TIM_PWM_Start(&DCM_TimeBaseStructure, TIM_CHANNEL_2);
}
 
/**
  * @brief  定时器更新事件回调函数
  * @param  无
  * @retval 无
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if (htim == (&TIM_EncoderHandle))
    {
        /* 判断当前计数器计数方向 */
        if (__HAL_TIM_IS_TIM_COUNTING_DOWN(&TIM_EncoderHandle))
            /* 下溢 */
        {
            Encoder_Overflow_Count--;
        }
        else
            /* 上溢 */
        {
            Encoder_Overflow_Count++;
        }
    }
}

ADC数据处理
static uint16_t flag_num = 0;
/**
  * @brief  常规转换在非阻塞模式下完成回调
  * @param  hadc: ADC  句柄.
  * @retval 无
  */
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef *hadc)
{
    uint32_t adc_mean = 0;
 
    HAL_ADC_Stop_DMA(hadc);       // 停止 ADC 采样,处理完一次数据在继续采样
 
    /* 计算电流通道采样的平均值 */
    for (uint32_t count = 0; count < 1024; count += 2)
    {
        adc_mean += (uint32_t)adc_buff[count];
    }
 
    adc_mean_sum += adc_mean / (1024 / 2);    // 保存平均值
    adc_mean_count++;
 
    adc_mean = 0;
 
    /* 计算电压通道采样的平均值 */
    for (uint32_t count = 1; count < 1024; count += 2)
    {
        adc_mean += (uint32_t)adc_buff[count];
    }
 
    vbus_adc_mean = adc_mean / (1024 / 2);    // 保存平均值
 
    HAL_ADC_Start_DMA(&ADC_Handle, (uint32_t *)&adc_buff, 1024); // 开始 ADC 采样
}
 
/**
  * @brief  在非阻塞模式模拟看门狗回调
  * @param  hadc: ADC  句柄.
  * @retval 无
  */
void HAL_ADC_LevelOutOfWindowCallback(ADC_HandleTypeDef *hadc)
{
    float temp_adc;
 
    flag_num++;     	// 电源电压超过阈值电压
 
    temp_adc = get_vbus_val();
 
    if (temp_adc > VBUS_MIN && temp_adc < VBUS_MAX)
    {
        flag_num = 0;
    }
 
    if (flag_num > 10) 	// 电源电压超过阈值电压10次
    {
        set_motor_disable();
        flag_num = 0;
        printf("电源电压超过限制!请检查原因,复位开发板在试!\r\n");
 
        while (1);
    }
}
 
/**
  * @brief  获取电流值(应定时调用)
  * @param  无
  * @retval 转换得到的电流值
  */
int32_t get_curr_val(void)
{
    static uint8_t flag = 0;
    static uint32_t adc_offset = 0;    				// 偏置电压
    int16_t curr_adc_mean = 0;         				// 电流 ACD 采样结果平均值
 
    curr_adc_mean = adc_mean_sum / adc_mean_count;  // 保存平均值
 
    adc_mean_count = 0;
    adc_mean_sum = 0;
 
    if (flag < 17 && is_motor_en == 0)				//	仅在电机未启动时记录
    {
        adc_offset = curr_adc_mean;    				// 多次记录偏置电压,待系统稳定偏置电压才为有效值
        flag += 1;
    }
 
    if (curr_adc_mean >= adc_offset)
    {
        curr_adc_mean -= adc_offset;                // 减去偏置电压
    }
    else
    {
        curr_adc_mean = 0;
    }
 
	float vdc = (float)curr_adc_mean/(float)65536 * 3.3f;	// 获取电压值
    return (float)vdc / 8.0f / 0.02f * 1000.0f;
}
 
/**
  * @brief  获取电源电压值
  * @param  无
  * @retval 转换得到的电流值
  */
float get_vbus_val(void)
{
    float vdc = (float)vbus_adc_mean/(float)65536 * 3.3f;	// 获取电压值
    return ((float)vdc - (float)1.24) * (float)37.0;		// 电源电压值(测量电压是电源电压的1/37)
}

编写位置式PID算法
typedef struct
{
    float target_val;	// 目标值
    float actual_val;	// 实际值
    float err;       	// 定义偏差值
    float err_last;  	// 定义上一个偏差值
    float Kp,Ki,Kd;  	// 定义比例、积分、微分系数
    float integral;  	// 定义积分值
}_pid;
_pid pid_location;		// 位置环控制
_pid pid_curr;			// 电流环控制
_pid pid_speed;			// 速度环控制

/**
  * @brief  PID参数初始化
	*	@note 	无
  * @retval 无
  */
void PID_param_init(void)
{
    /* 位置相关初始化参数 */
    pid_location.target_val = 0.0;
    pid_location.actual_val = 0.0;
    pid_location.err 		= 0.0;
    pid_location.err_last 	= 0.0;
    pid_location.integral 	= 0.0;
    pid_location.Kp 		= 0.011;
    pid_location.Ki 		= 0.0018;
    pid_location.Kd 		= 0.0;

    /* 速度相关初始化参数 */
    pid_speed.target_val 	= 100.0;
    pid_speed.actual_val 	= 0.0;
    pid_speed.err 			= 0.0;
    pid_speed.err_last 		= 0.0;
    pid_speed.integral 		= 0.0;
    pid_speed.Kp 			= 2.0;
    pid_speed.Ki 			= 0.02;
    pid_speed.Kd 			= 0.00;

    /* 电流相关初始化参数 */
    pid_curr.target_val 	= 80.0;
    pid_curr.actual_val 	= 0.0;
    pid_curr.err 			= 0.0;
    pid_curr.err_last 		= 0.0;
    pid_curr.integral 		= 0.0;
    pid_curr.Kp 			= 0.0;
    pid_curr.Ki 			= 3.5;
    pid_curr.Kd 			= 0.00;

    float pid_temp[3] = {pid_location.Kp, pid_location.Ki, pid_location.Kd};
    set_computer_value(SEND_P_I_D_CMD, CURVES_CH1, pid_temp, 3);     // 给通道 1 发送 P I D 值

    pid_temp[0] = pid_speed.Kp;
    pid_temp[1] = pid_speed.Ki;
    pid_temp[2] = pid_speed.Kd;
    set_computer_value(SEND_P_I_D_CMD, CURVES_CH2, pid_temp, 3);     // 给通道 2 发送 P I D 值

    pid_temp[0] = pid_curr.Kp;
    pid_temp[1] = pid_curr.Ki;
    pid_temp[2] = pid_curr.Kd;
    set_computer_value(SEND_P_I_D_CMD, CURVES_CH3, pid_temp, 3);     // 给通道 3 发送 P I D 值
}

/**
  * @brief  设置目标值
  * @param  val		目标值
	*	@note 	无
  * @retval 无
  */
void set_pid_target(_pid *pid, float temp_val)
{
    pid->target_val = temp_val;    // 设置当前的目标值
}

/**
  * @brief  获取目标值
  * @param  无
	*	@note 	无
  * @retval 目标值
  */
float get_pid_target(_pid *pid)
{
    return pid->target_val;    // 设置当前的目标值
}

/**
  * @brief  设置比例、积分、微分系数
  * @param  p:比例系数 P
  * @param  i:积分系数 i
  * @param  d:微分系数 d
	*	@note 	无
  * @retval 无
  */
void set_p_i_d(_pid *pid, float p, float i, float d)
{
    pid->Kp = p;    // 设置比例系数 P
    pid->Ki = i;    // 设置积分系数 I
    pid->Kd = d;    // 设置微分系数 D
}

/**
  * @brief  位置环位置式PID算法实现
  * @param  actual_val:实际值
  *	@note 	无
  * @retval 通过PID计算后的输出
  */
float location_pid_realize(_pid *pid, float actual_val)
{
    /*计算目标值与实际值的误差*/
    pid->err = pid->target_val - actual_val;

    /* 限定闭环死区 */
    if ((pid->err >= -40) && (pid->err <= 40))
    {
        pid->err = 0;
        pid->integral = 0;
    }

    /* 积分分离,偏差较大时去掉积分作用 */
    if (pid->err > -1500 && pid->err < 1500)
    {
        pid->integral += pid->err;    // 误差累积

        /* 限定积分范围,防止积分饱和 */
        if (pid->integral > 4000)
        {
            pid->integral = 4000;
        }
        else if (pid->integral < -4000)
        {
            pid->integral = -4000;
        }
    }

    /*PID算法实现*/
    pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);

    /*误差传递*/
    pid->err_last = pid->err;

    /*返回当前实际值*/
    return pid->actual_val;
}

/**
  * @brief  速度环位置式PID算法实现
  * @param  actual_val:实际值
  *	@note 	无
  * @retval 通过PID计算后的输出
  */
float speed_pid_realize(_pid *pid, float actual_val)
{
    /*计算目标值与实际值的误差*/
    pid->err = pid->target_val - actual_val;

    if ((pid->err < 0.2f) && (pid->err > -0.2f))
    {
        pid->err = 0.0f;
    }

    pid->integral += pid->err;    // 误差累积

    /*PID算法实现*/
    pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);

    /*误差传递*/
    pid->err_last = pid->err;

    /*返回当前实际值*/
    return pid->actual_val;
}

/**
  * @brief  电流环位置式PID算法实现
  * @param  actual_val:实际值
	*	@note 	无
  * @retval 通过PID计算后的输出
  */
float curr_pid_realize(_pid *pid, float actual_val)
{
    /*计算目标值与实际值的误差*/
    pid->err = pid->target_val - actual_val;

    pid->integral += pid->err;    // 误差累积

    /* 限定积分范围,防止积分饱和 */
    if (pid->integral > 2000)
    {
        pid->integral = 2000;
    }
    else if (pid->integral < -2000)
    {
        pid->integral = -2000;
    }

    /*PID算法实现*/
    pid->actual_val = pid->Kp * pid->err + pid->Ki * pid->integral + pid->Kd * (pid->err - pid->err_last);

    /*误差传递*/
    pid->err_last = pid->err;

    /*返回当前实际值*/
    return pid->actual_val;
}

#define TARGET_CURRENT_MAX    200    // 目标电流的最大值 mA
#define TARGET_SPEED_MAX      200    // 目标速度的最大值 r/m

/**
  * @brief  电机位置式 PID 控制实现(定时调用)
  * @param  无
  * @retval 无
  */
void motor_pid_control(void)
{
    static uint32_t louter_ring_timer = 0;      // 外环环周期(电流环计算周期为定时器周期T,速度环为2T,位置环为3T)
    int32_t actual_current = get_curr_val();    // 读取当前电流值

    if (actual_current > TARGET_CURRENT_MAX)
    {
        actual_current = TARGET_CURRENT_MAX;
    }

    if (is_motor_en == 1)                  		// 电机在使能状态下才进行控制处理
    {
        static int32_t Capture_Count = 0;    	// 当前时刻总计数值
        static int32_t Last_Count = 0;       	// 上一时刻总计数值
        float cont_val = 0;                  	// 当前控制值

        /* 当前时刻总计数值 = 计数器值 + 计数溢出次数 * ENCODER_TIM_PERIOD  */
        Capture_Count = __HAL_TIM_GET_COUNTER(&TIM_EncoderHandle) + (Encoder_Overflow_Count * 65535);

        /* 位置环计算 */
        if (louter_ring_timer++ % 3 == 0)
        {
            cont_val = location_pid_realize(&pid_location, Capture_Count);    // 进行 PID 计算

            /* 目标速度上限处理 */
            if (cont_val > TARGET_SPEED_MAX)
            {
                cont_val = TARGET_SPEED_MAX;
            }
            else if (cont_val < -TARGET_SPEED_MAX)
            {
                cont_val = -TARGET_SPEED_MAX;
            }

            set_pid_target(&pid_speed, cont_val);    // 设定速度的目标值

            int32_t temp = cont_val;
            set_computer_value(SEND_TARGET_CMD, CURVES_CH2, &temp, 1);     // 给通道 2 发送目标值
        }

        /* 速度环计算 */
        static int32_t actual_speed = 0;             // 实际测得速度

        if (louter_ring_timer % 2 == 0)
        {
            /* 转轴转速 = 单位时间内的计数值 / 编码器总分辨率 * 时间系数  */
            actual_speed = ((float)(Capture_Count - Last_Count) / 16 * 4 / 30) / (GET_BASIC_TIM_PERIOD() * 2 / 1000.0 / 60.0);

            /* 记录当前总计数值,供下一时刻计算使用 */
            Last_Count = Capture_Count;

            cont_val = speed_pid_realize(&pid_speed, actual_speed);    		// 进行 PID 计算

            if (cont_val > 0)    					// 判断电机方向
            {
                set_motor_direction(MOTOR_FWD);
            }
            else
            {
                cont_val = -cont_val;
                set_motor_direction(MOTOR_REV);
            }

            cont_val = (cont_val > TARGET_CURRENT_MAX) ? TARGET_CURRENT_MAX : cont_val;    // 电流上限处理
            set_pid_target(&pid_curr, cont_val);    						// 设定电流的目标值

            int32_t temp = cont_val;
            set_computer_value(SEND_TARGET_CMD, CURVES_CH3, &temp, 1);     	// 给通道 3 发送目标值
        }

        /* 电流环计算 */
        cont_val = curr_pid_realize(&pid_curr, actual_current);    			// 进行 PID 计算

        if (cont_val < 0)
        {
            cont_val = 0;    	// 下限处理
        }
        else if (cont_val > 5500)
        {
            cont_val = 5500;	// 速度上限处理
        }

        set_motor_speed(cont_val);                                                 // 设置 PWM 占空比

        set_computer_value(SEND_FACT_CMD, CURVES_CH1, &Capture_Count,  1);         // 给通道 1 发送实际值
    }
}

你可能感兴趣的:(#,电机应用开发,stm32)