二：使用正点原子的直流无刷驱动板自写FOC控制永磁同步(PMSM)电机（位置闭环）
在上一篇博客中配置了UVW三相PWM的定时器，在此基础上增加配置ABZ编码器定时器
启用一个定时器中断，用于PID处理
代码如下
// 常量定义 #define PI 3.14159265359f #define PWM_RESOLUTION 5250.0f // PWM分辨率 #define CIRCLE_RESOLUTION 1000 // 圆周分辨率 #define MOTOR_PP 4 // 电机极对数 #define MAX_MOTORS 2 // 最大电机数量 #define MAX_ANGULAR_VELOCITY 2.0f // 定义最大电角速度 // 电机参数结构体 typedef struct { float voltage_power_supply; // 电机电压 float zero_electric_angle; // 零电角位置 float dc_a, dc_b, dc_c; // 三相PWM占空比 float Ualpha, Ubeta, Ua, Ub, Uc; // 坐标变换后的电压 int32_t cur_pos; // 当前编码器位置 float rad_per_num; // 每个脉冲对应的电角度变化 uint8_t direction; // 电机转向 int32_t circle_count; // 圆周计数 // PI控制参数 float prev_angle_error, integral_angle_error; // 电角度PI控制误差 float prev_angular_velocity_error, integral_angular_velocity_error; // 电角速度PI控制误差 float Kp_angular_velocity, Ki_angular_velocity; // 电角速度PI控制增益 float outP_angular_velocity, outI_angular_velocity; // 电角速度PI控制输出 // Uq控制PI参数 float Kp_Uq, Ki_Uq; float max_Uq; // Uq最大值 // 目标角度和当前角度 float set_angle, cur_angle, tar_angle; // PWM和编码器的定时器 TIM_HandleTypeDef *pwm_timer; // PWM输出定时器 TIM_HandleTypeDef *enc_timer_ab; // 编码器A和B信号的定时器 TIM_HandleTypeDef *enc_timer_z; // 编码器Z信号的定时器 } Motor; // 全局电机实例数组 Motor motors[MAX_MOTORS]; // 函数原型声明 void setPwm(Motor *motor, float Ua, float Ub, float Uc); float getCurrentAngle(Motor *motor); float wrapAngle(float angle, float period); void moveToPosition(Motor *motor); void initFocMotor(int motor_idx, TIM_HandleTypeDef *pwm_timer, TIM_HandleTypeDef *enc_timer_ab, TIM_HandleTypeDef *enc_timer_z); void setPhaseVoltage(Motor *motor, float Uq, float Ud, float angle_el); // 约束函数：限制输入值在[min, max]范围内 float _constrain(float x, float min, float max) { return (x < min) ? min : (x > max) ? max : x; } // 电角度归一化函数：将角度归一化到[0, 2PI]区间 float _normalizeAngle(float angle) { float a = fmod(angle, 2 * PI); // 模运算 return a >= 0 ? a : (a + 2 * PI); // 如果角度为负，调整为正 } // 设置PWM输出函数：将电压转换为PWM占空比并输出 void setPwm(Motor *motor, float Ua, float Ub, float Uc) { // 根据电压计算PWM占空比，并约束在[0, 1]范围内 motor->dc_a = _constrain(Ua / motor->voltage_power_supply, 0.0f, 1.0f); motor->dc_b = _constrain(Ub / motor->voltage_power_supply, 0.0f, 1.0f); motor->dc_c = _constrain(Uc / motor->voltage_power_supply, 0.0f, 1.0f); // 将PWM占空比转换为实际的PWM值 uint32_t pwm_a = (uint32_t)(motor->dc_a * PWM_RESOLUTION); uint32_t pwm_b = (uint32_t)(motor->dc_b * PWM_RESOLUTION); uint32_t pwm_c = (uint32_t)(motor->dc_c * PWM_RESOLUTION); // 使用电机的PWM定时器设置PWM值 __HAL_TIM_SET_COMPARE(motor->pwm_timer, TIM_CHANNEL_1, pwm_a); __HAL_TIM_SET_COMPARE(motor->pwm_timer, TIM_CHANNEL_2, pwm_b); __HAL_TIM_SET_COMPARE(motor->pwm_timer, TIM_CHANNEL_3, pwm_c); } // 获取电机当前位置函数：根据编码器计数值计算电机位置 int32_t calculatePosition(Motor *motor) { int32_t count = __HAL_TIM_GET_COUNTER(motor->enc_timer_ab) / 4; int32_t position = count + (motor->circle_count * CIRCLE_RESOLUTION); return position; } // 获取当前电角度：根据编码器计数值计算电角度 float getCurrentAngle(Motor *motor) { // 获取编码器A信号的计数值并计算电角度 int32_t tim_count = __HAL_TIM_GET_COUNTER(motor->enc_timer_ab) / 4; return (float)(motor->rad_per_num * tim_count); // 每个计数对应的电角度 } void moveToPosition(Motor *motor) { motor->cur_angle = getCurrentAngle(motor); // 获取当前电角度 float angle_error = motor->tar_angle - motor->cur_angle; // 计算角度误差 // 电角速度PI控制 motor->outP_angular_velocity = motor->Kp_angular_velocity * angle_error; motor->outP_angular_velocity = _constrain(motor->outP_angular_velocity, -MAX_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY); // 限制PI输出 motor->outI_angular_velocity += motor->Ki_angular_velocity * angle_error; motor->outI_angular_velocity = _constrain(motor->outI_angular_velocity, -MAX_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY); // 限制PI积分输出 // 计算期望电角速度，并限制在[-MAX_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY]范围内 float desired_angular_velocity = motor->outP_angular_velocity + motor->outI_angular_velocity; desired_angular_velocity = _constrain(desired_angular_velocity, -MAX_ANGULAR_VELOCITY, MAX_ANGULAR_VELOCITY); // 控制Uq（磁场方向电压） float Uq = motor->Kp_Uq * angle_error + motor->Ki_Uq * motor->integral_angle_error; Uq = _constrain(Uq, -motor->max_Uq, motor->max_Uq); // 使用Uq最大值限制 Uq = fabs(Uq); // 保证Uq为正值 motor->set_angle = motor->cur_angle + desired_angular_velocity; // 设定目标电角度 setPhaseVoltage(motor, Uq, 0, motor->set_angle); // 设置相电压 } // 设置相电压：根据Uq和电角度计算相电压并输出PWM信号 void setPhaseVoltage(Motor *motor, float Uq, float Ud, float angle_el) { angle_el = _normalizeAngle(angle_el + motor->zero_electric_angle); // 归一化电角度 // 逆Park变换 motor->Ualpha = -Uq * sin(angle_el); motor->Ubeta = Uq * cos(angle_el); // 逆Clarke变换 motor->Ua = motor->Ualpha + motor->voltage_power_supply / 2.0f; motor->Ub = (sqrt(3.0f) * motor->Ubeta - motor->Ualpha) / 2.0f + motor->voltage_power_supply / 2.0f; motor->Uc = (-motor->Ualpha - sqrt(3.0f) * motor->Ubeta) / 2.0f + motor->voltage_power_supply / 2.0f; // 设置PWM输出 setPwm(motor, motor->Ua, motor->Ub, motor->Uc); } // 初始化FOC控制：启动PWM定时器和编码器定时器 // 通过传入电机索引和定时器参数来指定初始化哪个电机，并配置相关定时器 void initFocMotor(int motor_idx, TIM_HandleTypeDef *pwm_timer, TIM_HandleTypeDef *enc_timer_ab, TIM_HandleTypeDef *enc_timer_z) { // 检查电机索引是否有效 if (motor_idx >= MAX_MOTORS) { // 如果索引超出范围，返回错误 Error_Handler(); } // 初始化电机参数 motors[motor_idx].max_Uq = 8; // 设置Uq最大值,改变此值可以改变速度,理论最大值为母线电压的一半，即24/2 = 12，但是实际设为12电机会卡住，具体设为多少根据电机以及负载而定 motors[motor_idx].voltage_power_supply = 24.0f; // 设置电机电压 motors[motor_idx].zero_electric_angle = 0.0f; // 设置零电角度 motors[motor_idx].rad_per_num = MOTOR_PP * 2 * PI / CIRCLE_RESOLUTION * -1; // 每个脉冲对应的电角度变化 // 设置PI控制器参数 motors[motor_idx].Kp_angular_velocity = 1.0f; motors[motor_idx].Ki_angular_velocity = 0.01f; motors[motor_idx].Kp_Uq = 1.0f; motors[motor_idx].Ki_Uq = 0.0f; // 初始化定时器 motors[motor_idx].pwm_timer = pwm_timer; motors[motor_idx].enc_timer_ab = enc_timer_ab; motors[motor_idx].enc_timer_z = enc_timer_z; // 启动PWM输出（主输出和互补输出） if (HAL_TIM_PWM_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_1) != HAL_OK) Error_Handler(); if (HAL_TIMEx_PWMN_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_1) != HAL_OK) Error_Handler(); if (HAL_TIM_PWM_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_2) != HAL_OK) Error_Handler(); if (HAL_TIMEx_PWMN_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_2) != HAL_OK) Error_Handler(); if (HAL_TIM_PWM_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_3) != HAL_OK) Error_Handler(); if (HAL_TIMEx_PWMN_Start(motors[motor_idx].pwm_timer, TIM_CHANNEL_3) != HAL_OK) Error_Handler(); // 启动编码器定时器AB（ENCA和ENCB） if (HAL_TIM_Encoder_Start(motors[motor_idx].enc_timer_ab, TIM_CHANNEL_ALL) != HAL_OK) Error_Handler(); // 启动编码器定时器Z（ENCZ） if (HAL_TIM_IC_Start(motors[motor_idx].enc_timer_z, TIM_CHANNEL_2) != HAL_OK) Error_Handler(); } // 编码器中断回调函数 void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) { // 获取电机1的编码器信息 if (htim->Instance == motors[0].enc_timer_z->Instance && htim->Channel == HAL_TIM_ACTIVE_CHANNEL_2) { // 获取电机当前的方向 motors[0].direction = (htim3.Instance->CR1 & TIM_CR1_DIR) ? 0 : 1; // 根据方向更新转向次数 if (motors[0].direction) { motors[0].direction++; } else { motors[0].direction--; } // 这里可以选择是否需要清零计数器 // __HAL_TIM_SET_COUNTER(&htim3, 0); // 重置ENCA和ENCB的计时器 } } // 定时器溢出回调函数 void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) { // 处理电机控制定时器溢出 if (htim->Instance == TIM6) { // 判断是否是TIM6中断，周期为1ms moveToPosition(&motors[0]); // 调用电机控制函数 } } int main(void) { // 初始化电机0的FOC控制，传入对应的定时器 initFocMotor(0, &htim1, &htim3, &htim9); // 强制将电角度保持在0 setPhaseVoltage(&motors[0], 1.0f, 0.0f, 0.0f); osDelay(100); // 重置编码器计数器 __HAL_TIM_SET_COUNTER(motors[0].enc_timer_ab, 0); // 启动FOC控制的定时器 HAL_TIM_Base_Start_IT(&htim6); motors[0].tar_angle = -2 * PI;//设置电机到-2pi的电角度位置，如果目标电角度值在0到-8pi之间，处于位置模式。小于-8pi(一个机械周期，电机极对数为4，电角度 = 4 * 2pi)电机就会一直旋转。 while(1) { } }
按一下复位键移动-2PI电角度，效果如下
电角度小于-8PI,就会旋转
