直流无刷减速电机PID控制
最近做了直流无刷减速电机的控制的项目，针对项目中遇到的问题做下总结。
#PID_Control
PID 代码（速度环 位置环 串级）
STM32F407VET6
STM32CubeMX
更新记录
V1.0.0 2022/8/5
- 完善了RTOS程序模板，包括RTT功能.
- 完成了位置PID 速度环控制.
- 增加了上位机串口通信协议，可用上位机调节PID参数.
V1.0.1 2022/8/8
- 实现了通过M法测速 将单位时间内编码器的脉冲数转换为速度，
速度分两个 a、电机转子的转速 b、减速器输出轴转速
b = a / REDUCTION_RATIO(减速比).
/* 转轴转速 = 单位时间内的计数值 / 编码器总分辨率 * 时间系数 */
Shaft_Speed = ((float)(pitch_cfg.cnt)) / (((float)(ENCODER_TOTAL_RESOLUTION)) * 0.01f);
2、需要理清楚几个概念：
1、每转脉冲数 PPR 圆周分辨率，Pulse Per Revolution 或 PPR （分辨率）
2、每转计数 CPR CPR = PPR * 4
3、RPM 每分钟转速
例如,某旋转编码器的分辨率为100PPR,表示每旋转一圈,该编码器可输出100个脉冲信号;
同理,某旋转编码器的分辨率为256PPR,则表示每旋转一圈的脉冲数为256个.
3、设置PID的目标值 为转轴转速 来实现调速，减速器输出轴转速太小了，上位机不支持.
V1.0.2 2022/8/11
- 增加了对电机位置的控制，通过控制脉冲数控制电机位置
/* 电机转一圈的脉冲数*/
#define PULSE_SUM 124264=12672
V1.0.3 2022/8/16
- 增加了串级PID，位置环和速度环
/* 电机转一圈的脉冲数*/
#define PULSE_SUM 12672 // htim3.Init.Prescaler = 0，理论上最多可转4圈
#define PULSE_SUM_FOUR 3168 // htim3.Init.Prescaler = 4-1 ,相当于之前1us采集一次编码器脉冲，现在4us采集一次，总采集数为65535 一圈脉冲数为 3168，理论上最多可转20圈，而且速度按最大值进行旋转，所以位置PID速度不可控。
/* Private define ------------------------------------------------------------*/
/* 编码器接口倍频数 */
#define ENCODER_MODE TIM_ENCODERMODE_TI12
#define PITCH_TICK2DEG 0.028869198 //TT电机
#define PITCH_MOTOR_MAX_OUTPUT 400 //扭矩
#define YAW_MOTOR_MAX_OUTPUT 400
#define DOWN_LIMIT_OFFSET -200
#define ENCODER_TIM_PERIOD 65535 /*计数器最大值*/
/* 编码器物理分辨率 */
#define ENCODER_RESOLUTION 12 //PPR
/* 减速电机减速比 */
#define REDUCTION_RATIO 264
/* 电机转一圈的脉冲数*/
#define PULSE_SUM 12672 // htim3.Init.Prescaler = 0，理论上最多可转4圈
#define PULSE_SUM_FOUR 3168 // htim3.Init.Prescaler = 4-1 ,相当于之前1us采集一次编码器脉冲，现在4us采集一次，
// 总采集数为65535 一圈脉冲数为 3168，理论上最多可转20圈，而且速度按最大值进行旋转，所以位置PID速度不可控。
/* 经过倍频之后的总分辨率 */
#if (ENCODER_MODE == TIM_ENCODERMODE_TI12)
#define ENCODER_TOTAL_RESOLUTION (ENCODER_RESOLUTION * 4) /* 4倍频后的总分辨率 CPR*/
#else
#define ENCODER_TOTAL_RESOLUTION (ENCODER_RESOLUTION * 2) /* 2倍频后的总分辨率 */
#endif
/***
* @brief bsp_motor_get_position
* @param None
* @retval None
*/
int16_t bsp_motor_get_position(motor_id_e id)
{
int32_t pos = 0;
if(id == YAW_MOTOR)
{
pos = -(int16_t)__HAL_TIM_GET_COUNTER(&htim4);
//pos = (int16_t)__HAL_TIM_GET_COUNTER(&htim3);
}
else if(id == PITCH_MOTOR)
{
//pos = (int32_t)__HAL_TIM_GET_COUNTER(&htim2);
pos = (int16_t)__HAL_TIM_GET_COUNTER(&htim1);
}
return pos;
}
一、速度环PID
/***
* @brief pitch_test
* @param None
* @retval None
*/
/* 电机转轴转速 */
__IO float Shaft_Speed = 0.0f;
void vPitch_Task(void const * argument)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
//set_p_i_d(15.0, 2.0, 0.0);
//set_pid_target(30.0);
static int count = 0;
int test = 1;
int pid_out = 0;
pitch_cfg.pwm = 0;
pitch_cfg.run2up = 1;
while(test)
{
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10));
if(pitch_cfg.run2up)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2UP);//向上运动
//bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));
}
else
{
bsp_motor_set_direction(PITCH_MOTOR, HD2DOWN);//向上运动
}
pitch_cfg.cnt = bsp_motor_get_position(PITCH_MOTOR);
DBG_Printf("%s: Encoder:%d \r\n", __FUNCTION__, pitch_cfg.cnt);
/* 转轴转速 = 单位时间内的计数值 / 编码器总分辨率 * 时间系数 */
Shaft_Speed = ((float)(pitch_cfg.cnt)) / (((float)(ENCODER_TOTAL_RESOLUTION)) * 0.01f);
DBG_Printf("Speed of motor shaft:%f r/s \r\n", Shaft_Speed); //电机转子的转速
//DBG_Printf("SPeed of motor output shaft:%f r/s \r\n", Shaft_Speed / REDUCTION_RATIO); //减速器输出轴的转速
uint32_t speed_temp = 0;
speed_temp = (uint32_t)Shaft_Speed;
#if (PID_ASSISTANT_EN)
count++;
if (count==8)
{
count = 0;
set_computer_value(SEND_FACT_CMD, CURVES_CH1, &speed_temp , 1); // 给通道 1 发送实际值
}
#endif
pid_out = (int)PID_realize(speed_temp);
DBG_Printf("%s: PID_OUT:%d \r\n", __FUNCTION__, pid_out);
pitch_cfg.pwm = pwm_val_protect(pid_out);
DBG_Printf("%s: PWM:%d \r\n", __FUNCTION__, pitch_cfg.pwm);
DBG_Printf("\r\n");
bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));
}
}
二、位置环PID
/***
* @brief
* pitch_test
* @param None
* @retval None
*/
/* 电机转轴转速 */
__IO float Shaft_Speed = 0.0f;
extern __IO int16_t EncoderOverflowCnt;
void vPitch_Task(void const * argument)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
__IO int32_t encoderNow = 0; /*当前时刻总计数值*/
//set_p_i_d(15.0, 2.0, 0.0);
//set_pid_target(30.0);
static int count = 0;
int test = 1;
int pid_out = 0;
pitch_cfg.pwm = 0;
pitch_cfg.run2up = 1;
while(test)
{
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10));
if(pitch_cfg.run2up)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2UP);//向上运动
//bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));
}
else
{
bsp_motor_set_direction(PITCH_MOTOR, HD2DOWN);//向上运动
}
DBG_Printf("\r\n");
pitch_cfg.cnt = bsp_motor_read_encoder(PITCH_MOTOR);
DBG_Printf("%s: Encoder:%d \r\n", __FUNCTION__,pitch_cfg.cnt);
DBG_Printf("%s:flowCnt:%d \r\n", __FUNCTION__,EncoderOverflowCnt);
/*【1】读取编码器的值*/
encoderNow = pitch_cfg.cnt + EncoderOverflowCnt * ENCODER_TIM_PERIOD;/*获取当前的累计值*/
DBG_Printf("%s: encoder_Now:%d \r\n", __FUNCTION__, encoderNow);
/*【2】PID运算，得到PWM控制值*/
pid_out = (int)PID_realize(encoderNow);
DBG_Printf("%s: pid_out:%d \r\n", __FUNCTION__, pid_out);
pitch_cfg.pwm = pwm_val_protect(pid_out);
/*【3】PWM控制电机*/
DBG_Printf("%s: pwm:%d \r\n", __FUNCTION__, pitch_cfg.pwm);
uint32_t value_tt = 0;
if (pid.target_val > 0)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2UP);//向上运动
bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));/*传入编码器的[总计数值]，实现电机【位置】控制*/
}
else if (pid.target_val < 0)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2DOWN);//向下运动
bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));/*传入编码器的[总计数值]，实现电机【位置】控制*/
}
else
{
bsp_motor_set_speed(PITCH_MOTOR, &value_tt);
}
/*【4】数据上传到上位机显示*/
#if (PID_ASSISTANT_EN)
count++;
if (count==8)
{
count = 0;
int Temp = encoderNow;
set_computer_value(SEND_FACT_CMD, CURVES_CH1, &Temp , 1); // 给通道 1 发送实际值
}
#endif
}
}
三、串级PID
/***
* @brief pitch_test
* @param None
* @retval None
*/
/* 电机转轴转速 */
extern __IO int16_t EncoderOverflowCnt;
void vPitch_Task(void const * argument)
{
TickType_t xLastWakeTime = xTaskGetTickCount();
int encoderNow = 0; /*当前时刻总计数值*/
static uint32_t location_timer = 0; // 位置环周期
static __IO int encoderLast = 0; /*上一时刻总计数值*/
int encoderDelta = 0; /*当前时刻与上一时刻编码器的变化量*/
static int count = 0;
int32_t actual_speed = 0.0f;
int test = 1;
uint32_t value_tt = 0;
int actual_speed_int = 0;
pitch_cfg.pwm = 0;
while(test)
{
vTaskDelayUntil(&xLastWakeTime, pdMS_TO_TICKS(10));
DBG_Printf("\r\n");
pitch_cfg.cnt = bsp_motor_read_encoder(PITCH_MOTOR);
DBG_Printf("%s: Encoder:%d \r\n", __FUNCTION__,pitch_cfg.cnt);
DBG_Printf("%s:flowCnt:%d \r\n", __FUNCTION__,EncoderOverflowCnt);
/*【1】读取编码器的值*/
encoderNow = pitch_cfg.cnt + EncoderOverflowCnt * ENCODER_TIM_PERIOD;/*获取当前的累计值*/
/* 计算速度环的传入值 */
encoderDelta = encoderNow - encoderLast; /*得到变化值 速度环的传入值*/
encoderLast = encoderNow;/*更新上次的累计值*/
DBG_Printf("%s: encoderNow:%d \r\n", __FUNCTION__, encoderNow);
DBG_Printf("%s: encoderDelta:%d \r\n", __FUNCTION__, encoderDelta);
/*【2】位置PID运算，得到速度目标值*/
if ((location_timer++ % 2) == 0)
{
float control_val = 0; /*当前控制值*/
/*位置PID计算*/
control_val = location_pid_realize(&pid_location, encoderNow);
DBG_Printf("%s: speed_current:%.2f \r\n", __FUNCTION__, control_val);
/*目标速度值限制*/
speed_val_protect(&control_val);
/*设定速度PID的目标值*/
set_pid_target(&pid_speed, control_val);
DBG_Printf("%s: pid_speed_target:%.2f \r\n", __FUNCTION__, control_val);
#if defined(PID_ASSISTANT_EN)
if ((location_timer % 16) == 8)
{
int temp_speed = 0;
temp_speed = (int)control_val;
set_computer_value(SEND_TARGET_CMD, CURVES_CH2, &temp_speed, 1); // 给通道2发送速度目标值
}
#endif
}
/*【3】速度PID运算，得到PWM控制值*/
actual_speed = (int32_t)((encoderDelta) / (ENCODER_TOTAL_RESOLUTION*0.01f));
DBG_Printf("%s: actual_speed:%d \r\n", __FUNCTION__, actual_speed);
actual_speed_int = actual_speed;
pitch_cfg.pwm = pwm_val_protect((int)speed_pid_realize(&pid_speed, actual_speed));
DBG_Printf("%s: actual_pwm:%d \r\n", __FUNCTION__, pitch_cfg.pwm);
if (pid_speed.target_val > 0)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2UP);//向上运动
bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));/*传入编码器的[总计数值]，实现电机【位置】控制*/
}
else if (pid_speed.target_val < 0)
{
bsp_motor_set_direction(PITCH_MOTOR, HD2DOWN);//向下运动
bsp_motor_set_speed(PITCH_MOTOR, &(pitch_cfg.pwm));/*传入编码器的[总计数值]，实现电机【位置】控制*/
}
else
{
bsp_motor_set_speed(PITCH_MOTOR, &value_tt);
}
/*【4】数据上传到上位机显示*/
#if (PID_ASSISTANT_EN)
count++;
if(count%12 == 5)
{
set_computer_value(SEND_FACT_CMD, CURVES_CH1, &encoderNow, 1); /*给通道1发送实际的电机【位置】值*/
}
else if(count%12 == 10)
{
set_computer_value(SEND_FACT_CMD, CURVES_CH2, &actual_speed_int, 1); /*给通道2发送实际的电机【速度】值*/
}
#endif
}
}