[STM32]基于PID实现自动调速

增量式pid公式对于我们来说不需要微分项只需要调Kp和Ki就够了。#include motor.h #include main.h #include tim.h #include gpio.h void motor_init(uint8_t motor_id) { if(motor_id 1) { //A1N1-H A1N2-H STBY-H 制动 HAL_GPIO_WritePin(GPIOA,STBY_Pin,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOA,A1N1_Pin,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOA,A1N2_Pin,GPIO_PIN_SET); //启动pwm HAL_TIM_PWM_Start(htim2,TIM_CHANNEL_1); //ccr-0 __HAL_TIM_SET_COMPARE(htim2,TIM_CHANNEL_1,0); //启动编码器A,B相 HAL_TIM_Encoder_Start(htim3,TIM_CHANNEL_1); HAL_TIM_Encoder_Start(htim3,TIM_CHANNEL_2); } else if(motor_id 2) { } //以中断方式启动定时器 HAL_TIM_Base_Start_IT(htim4); } //duty范围:0-99 void motor_set_duty(uint8_t motor_id,uint16_t duty) { if(motor_id 1) { //ccr - duty __HAL_TIM_SET_COMPARE(htim2,TIM_CHANNEL_1,duty); } else if(motor_id 2) { } } //direction: 0-正转 1-反转 void motor_set_direction(uint8_t motor_id,uint8_t direction) { if(motor_id 1) { if(direction 1) { HAL_GPIO_WritePin(GPIOA,A1N1_Pin,GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOA,A1N2_Pin,GPIO_PIN_RESET); } else if(direction 0) { HAL_GPIO_WritePin(GPIOA,A1N1_Pin,GPIO_PIN_RESET); HAL_GPIO_WritePin(GPIOA,A1N2_Pin,GPIO_PIN_SET); } } else if(motor_id 2) { } } // 定义各电机的速度缓存 float motor_speed_cache[2] {0.0f, 0.0f}; /** * brief 获取指定电机的实时速度单位mm/s * param motor_id 电机编号1或2 * retval 无 */ void motor_speed(uint8_t motor_id) { if (motor_id 1) { // 读取编码器定时器TIM3当前计数值16位范围0~65535 uint32_t current __HAL_TIM_GET_COUNTER(htim3); // 公式v (Δ脉冲 / 编码器线数) * (π * 轮胎直径) / T motor_speed_cache[0] (float)current / encoder_xianShu * (pi * tire_diameter) / 0.03f; //cnt置0 __HAL_TIM_SET_COUNTER(htim3,0); } else if (motor_id 2) { } } float Kp 0.5; //比例系数 float Ki 0.4; //积分系数 uint16_t pwm_1_duty 0; float target_speed_1 0; //目标速度 mm/s float last_errol_1 0; //上一次误差 float current_errol_1 0; //当前误差 void motor_pid(uint8_t motor_id) { float errol 0; if(motor_id 1) { //计算e(k) errol target_speed_1 - motor_speed_cache[0]; current_errol_1 errol; //pid增量公式 Kp * [e(k) - e(k-1)] Ki * e(k) pwm_1_duty (uint16_t)(Kp * (current_errol_1 - last_errol_1) Ki * current_errol_1); //记录e(k-1) last_errol_1 current_errol_1; //输出对应pwm motor_set_duty(1,pwm_1_duty); } else if(motor_id 2) { } } /** * brief 定时器4周期中断回调函数每30ms执行一次 * param htim 定时器句柄 * retval 无 */ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // 判断是否为定时器4的中断用于速度采样周期 if (htim htim4) { motor_speed(1); motor_pid(1); } }