信息系统的网站开发答辩问题,wordpress related posts,媒介盒子,wordpress 采集功能目录 stm32cubemx配置芯片选择工程配置stm32基础配置SPI的配置定时器的配置ADC的配置中断优先级的配置生成工程 工程代码编写FOC代码结构搭建电机编码器角度读取PWM产生FOC开环代码编写确定电机正负旋转方向电机旋转速度计算多圈逻辑角度电流采样极对数转子角度确定 闭环控制控… 目录 stm32cubemx配置芯片选择工程配置stm32基础配置SPI的配置定时器的配置ADC的配置中断优先级的配置生成工程 工程代码编写FOC代码结构搭建电机编码器角度读取PWM产生FOC开环代码编写确定电机正负旋转方向电机旋转速度计算多圈逻辑角度电流采样极对数转子角度确定 闭环控制控制函数接口定义CMSIS-DSP提供的PID控制器位置控制速度控制力矩电流控制位置-速度-力矩控制PID系数调节 注意点 本节使用stm32cubemx配置外设生成keil工程代码适配本文的硬件电路板可参考上节的硬件设计内容。
点击查看本文开源的完整FOC工程https://gitee.com/best_pureer/stm32_foc。
stm32cubemx配置
芯片选择
选择stm32f103c8t6后点击Start Project。
工程配置
切换到Projeck Manager页面设定好项目名称、项目路径选择生成keil工程。
stm32基础配置
配置时钟
这里选择晶振 切换到Clock Configuration页面首先选择PLLCLK再HCLK输入72再点击确认会自动生成72MHz的主频。
SWD接口配置
用于烧录和调试程序
串口配置
用于打印一些调试信息选择串口2因为串口1会被定时器1的pwm通道占用
LED灯配置 CMSIS-DSP数学库配置
第一步 第二步
SPI的配置
用于读取MT6701这里的SPI波特率不要设置太高因为stm32f103c8t6的计算能力有限角度读的太快算不过来。 从MT6701的数据手册中可以看到应该在CLK的每2个脉冲时读取DO的数据而且CLK空闲电平是低电平因此设置CPOL为LowCPHA为2个边沿。 由于D13先发送因此是先高位传输即MSB大端传输方式。 这里将SPI的模式设置为全双工Full-Duplex Master而不设置为只读取模式Receive Only Master。虽然单片机的SPI只需要读取MT6701的数据无需SPI写入数据但是实测设置为只读取模式时貌似是每读完1个字节进一次读取完成中断而我们想要的是每读完3个字节进一次读取完成中断全双工模式能做到这个效果。 注意不要使用硬件片选NSS对应芯片CSN读取使能引脚因为硬件片选NSS是单片机disable对应SPI外设后才会拉低。 这里选择手动操作SPI的片选引脚设置片选信号所在引脚为GPIO输出模式操作逻辑是每次在SPI读取MT6701前拉低该引脚在刚进入读取完成中断时拉高该引脚。 后续使用DMA读取SPI数据全双工模式下接收和发送DMA均需要打开
定时器的配置
用于电机速度计算的定时器
首先勾选时钟来自内部时钟源再创建一个宏定义用于速度计算间隔。motor_speed_calc_freq是FOC代码里手动设置的宏speed_calc_freq这个宏给cubemx用这样就将cubemx的参数与FOC代码里的宏绑定到了一起好处就是FOC代码与cubemx独立开FOC代码用于其他厂商单片机不用改太多的代码。 首先设置分频为72-172MHz的时钟分频下来后每1us定时器计数加1。定时器计数容量为N时定时器溢出中断频率是1秒/N微妙1000000/N赫兹我们想要定时器溢出中断频率等于motor_speed_calc_freq那么需要1000000/Nmotor_speed_calc_freq因此这里的计数容量设置为N1000000/motor_speed_calc_freq。 由于cubemx是不认识motor_speed_calc_freq的因此这里选择No check不检查参数类型。 开启定时器中断函数用于计算电机速度
用于产生电机PWM的高级定时器
高级定时器的使用非常关键非常重要对于外设配置项的说明请查看前文定时器章节。 stm32f103c8t6只有一个高级定时器TIM1。 首先选择时钟源然后开启3个PWM正通道由于本文使用的集成驱动芯片DRV8313自带互补PWM功能因此不开启PWM负通道。 这里注意不勾选Activate-Break-Input这个只是帮你配置刹车引脚的GPIO参数刹车功能依然是有效的。不勾选的原因是经过我的各种尝试触发刹车后依然进不去刹车中断函数TIM1 break interrupt所以选择自己配置刹车引脚GPIO为外部中断。 手动设置刹车引脚为外部中断这里不设置也没事本文代码是因为想要触发刹车后串口打印一下告知刹车被触发了 开启外部中断函数 回到高级定时器的配置再次提醒对于外设配置项的说明请查看前文定时器章节。
PWM的频率是非常高的我设置的20KHz因此不进行预分频。计数方式选择中心对齐方式3。计数容量定义了一个cubemx宏与FOC代码里的宏进行绑定这样做可以使得FOC代码独立在各种平台上使用勾选No check不检查数值类型。数值计算过程定时器计数不分频情况下每 1 72 ∗ 1000000 \frac{1}{72*1000000} 72∗10000001秒计数加1计数容量为N时计数溢出时间为 N 72000000 \frac{N}{72000000} 72000000N则计数溢出频率为 72000000 N \frac{72000000}{N} N72000000而我们设定的计数溢出频率motor_pwm_freq即 72000000 N m o t o r _ p w m _ f r e q \frac{72000000}{N}motor\_pwm\_freq N72000000motor_pwm_freq因此计数容量 N 72000000 m o t o r _ p w m _ f r e q N\frac{72000000}{motor\_pwm\_freq} Nmotor_pwm_freq72000000。重复计数器设置为4即每5个定时器溢出产生1个更新事件更新事件会触发ADC采样本文FOC代码是放在ADC采样完成中断里计算的这样FOC代码的计算频率等于20KHz/54KHz这个频率不要太快因为stm32f103c8t6的算力有限计算一次FOC代码大约需要120us。输出触发事件来源选择更新事件自动触发ADC采样。BRK State刹车功能可以开启也可以关闭建议验证FOC算法阶段先关闭本文配套硬件有刹车引脚LED灯提示。对应DRV8313的Fault引脚信号刹车电平选择Low低电平。3个PWM Generation Channel的配置项都相同默认值刚好是我们想要配置的值。关键项是Mode选择PWM mode 1因为DRV8313内部是NMOS所以CH Polarity有效电平是高电平。
ADC的配置
ADC的使用非常关键非常重要对于外设配置项的说明请查看前文ADC章节。 使用双ADC同步采样两条电机相线上采样电阻放大后的电压差首先给ADC1和ADC2分配通道IN0对应PA0引脚IN0对应PA1引脚ADC1是主ADC。 两个ADC都分配好通道后主ADC的模式下拉框里才会出现同步采样的选项由于只有注入式采样才能绑定到高级定时器的触发事件因此选择注入式同步采样。 接下来配置ADC选项
同步采样模式下一定要开启Continuous连续转换模式。关闭常规采样打开注入采样。每个ADC只采集一个通道因此Number Of Conversions设置为1ADC1的Rank序列中的Channel通道选择通道0ADC2的Rank序列中的Channel通道选择通道1采样时间可以设置大一点我们代码中限制PWM占空比最高为90%即可留出充足的时间给ADC采样。外部触发源选择定时器1的触发事件。 开启ADC采样完成中断FOC代码会放在这个中断函数里进行
中断优先级的配置
读取磁编码器SPI的DMA中断优先级设置到最优先电机角度获取一定要及时准确。 接下来速度计算的定时器中断和运行FOC代码的ADC中断其次速度计算和FOC代码必须尽量优先。 连到刹车引脚的外部中断优先级放到最低即可刹车功能在刹车引脚有效时被硬件触发和中断无关这个外部中断只是为了串口打印几句消息告知触发刹车了。
生成工程
至此stm32cubemx相关配置已经完毕接下来生成keil工程这里勾选了Generate peripheral initialization as a pair of .c/.h files per peripheral方便功能模块分离。
此时keil工程编译是无法通过的编译器可能没选到verison 6而且两个宏motor_speed_calc_freq和motor_pwm_freq我们还没写到代码里。 首先选择编译器为version 6如果没有出现version 6你需要下载最新keil版本。 接下来我会使用vscode进行代码编辑keil用来编译工程和烧录调试代码。 接下来定义motor_speed_calc_freq和motor_pwm_freq。
首先在Drivers文件夹下创建motor文件夹这个文件夹专门用来放置电机FOC驱动代码。然后motor文件夹下创建conf.h这个文件专门用来放置FOC代码相关的工程配置将Drivers文件夹添加到头文件路径中。最后在main.h文件的用户代码区把conf.h文件include进来即可将工程编译通过。
在Src/usart.c中定义fputc方便后续使用printf函数
//usart.c
/* USER CODE BEGIN 1 */
#include stdio.h
int fputc(int c, FILE *stream)
{uint8_t ch[] {(uint8_t)c};HAL_UART_Transmit(huart2, ch, 1, HAL_MAX_DELAY);return c;
}
/* USER CODE END 1 */在Inc文件夹下创建一个全局宏定义文件global_def.h方便调用
//global_def.h
#pragma once
#ifndef PI
#define PI 3.14159265358979
#endif
#define deg2rad(a) (PI * (a) / 180)
#define rad2deg(a) (180 * (a) / PI)
#define max(a, b) ((a) (b) ? (a) : (b))
#define min(a, b) ((a) (b) ? (a) : (b))由于本文使用的daplink是低成本版本为了保证传输稳定性下载程序的频率选择为1MHz。
工程代码编写
在stm32hal库中应用逻辑的代码实现变得非常方便基本规律就是外设读取读取完成中断。
FOC代码结构搭建
根据本文硬件情况补充FOC代码里的conf.h
#pragma once// 电机物理参数
#define POLE_PAIRS 7 // 极对数// 电路参数
#define R_SHUNT 0.02 // 电流采样电阻欧姆
#define OP_GAIN 50 // 运放放大倍数
#define MAX_CURRENT 2 // 最大q轴电流安培A
#define ADC_REFERENCE_VOLT 3.3 // 电流采样adc参考电压伏
#define ADC_BITS 12 // ADC精度bit// 单片机配置参数
#define motor_pwm_freq 20000 // 驱动桥pwm频率Hz
#define motor_speed_calc_freq 930 // 电机速度计算频率Hz// 软件参数
#define position_cycle 6 * 3.14159265358979 // 电机多圈周期等于正半周期负半周期在Drivers/motor文件夹下创建motor_runtime_param.c和motor_runtime_param.h用于放置电机运行过程中的各种参数。 关于转子角度和电机角度的区别前文位置控制说明了一部分后面内容有代码上的讲解。
//motor_runtime_param.c
#include motor_runtime_param.hfloat motor_i_u;
float motor_i_v;
float motor_i_d;
float motor_i_q;
float motor_speed;
float motor_logic_angle;
float encoder_angle;
float rotor_zero_angle;//motor_runtime_param.h
#pragma once
#include conf.h
#define rotor_phy_angle (motor_logic_angle - rotor_zero_angle) // 转子物理角度
#define rotor_logic_angle rotor_phy_angle *POLE_PAIRS // 转子多圈角度
extern float motor_i_u;
extern float motor_i_v;
extern float motor_i_d;
extern float motor_i_q;
extern float motor_speed;
extern float motor_logic_angle; // 电机多圈角度
extern float encoder_angle; // 编码器直接读出的角度
extern float rotor_zero_angle; // 转子d轴与线圈d轴重合时的编码器角度在Drivers/motor文件夹下创建foc.c和foc.h准备用于放置FOC代码。 添加好头文件路径 添加好C文件
电机编码器角度读取
从MT6701数据手册可知SPI数据一共有3个字节 采用DMA方式SPI读取3个字节再取前14位组合成角度值这里我不使用4bit的Status信息也不进行crc校验这不影响角度读取你可自行运用这些数据提高程序健壮性。 在Src/spi.c中的USER CODE区域实现读取成功的回调函数还要实现读取失败的回调函数每次进入读取完毕函数后立即关闭SPI片选引脚并且在即将退出回调函数的时候打开SPI片选引脚并启动下一次读取。
//spi.c
/* USER CODE BEGIN 1 */
#include stdio.h
#include motor/motor_runtime_param.h
#include motor/foc.h
#include arm_math.h
uint8_t mt6701_rx_data[3];
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{if (hspi-Instance SPI1){HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);int angle_raw (mt6701_rx_data[1] 2) | (mt6701_rx_data[0] 6);encoder_angle 2 * 3.1415926 * angle_raw / ((1 14) - 1);HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);HAL_SPI_TransmitReceive_DMA(hspi1, mt6701_rx_data, mt6701_rx_data, 3);return;}
}void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
{printf(HAL_SPI_ErrorCallback:%d\n, hspi-ErrorCode);if (hspi-Instance SPI1){HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);HAL_SPI_TransmitReceive_DMA(hspi1, mt6701_rx_data, mt6701_rx_data, 3);}return;
}
/* USER CODE END 1 */在main函数的while(1)之前调用一次DMA方式的SPI读取即可自动连续获取角度值然后在while(1)里面打印一下角度验证一下代码手动旋转一下电机结果会在 0 0 0到 2 π 2\pi 2π弧度值之间变化
//main.c
//......
/* USER CODE BEGIN Includes */
#include stdio.h
#include motor/motor_runtime_param.h
#include motor/foc.h
#include global_def.h
/* USER CODE END Includes */
//......
/* USER CODE BEGIN 2 */
extern uint8_t mt6701_rx_data[3];
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);
HAL_SPI_TransmitReceive_DMA(hspi1, mt6701_rx_data, mt6701_rx_data, 3);
HAL_Delay(100);//延时一会让角度变量被赋值不然角度会是0
/* USER CODE END 2 */
//......
while(1)
{
//....../* USER CODE BEGIN 3 */printf(%f\n, encoder_angle);HAL_Delay(100);
//......在此再提醒读取角度的SPI中断优先级一定要最高否则片选信号可能没有及时关闭导致角度读取无法接续。
PWM产生
开启PWM输出
//main.c
//......HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1);HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2);HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_3);/* USER CODE END 2 */
//......自己定义一个函数set_pwm_duty方便设置PWM占空比。htim1.Instance-ARR是定时器计数容量。 有两个细节 限制占空比最高为90%留出一个电流稳定的时段有利于减少电机抖动以及给后续ADC采样提供稳定电流时段。原因是占空比接近100%时会出现mos关闭后瞬间开启的情况而且此时是电流通过导致电流非常不稳定不必要限制最低占空比因为q轴为0时由于SVPWM中的000矢量和111矢量两个零力矩矢量平分了一个PWM周期因此FOC控制下的PWM最低占空比就是50%。 操作3个通道时关闭了中断防止通道不同步。
//main.c
/* USER CODE BEGIN 0 */
void set_pwm_duty(float d_u, float d_v, float d_w)
{d_u min(d_u, 0.9);d_v min(d_v, 0.9);d_w min(d_w, 0.9);__disable_irq();__HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, d_u * htim1.Instance-ARR);__HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, d_v * htim1.Instance-ARR);__HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_3, d_w * htim1.Instance-ARR);__enable_irq();
}
/* USER CODE END 0 */FOC开环代码编写
FOC开环就是实现一下SVPWM代码输入目标d轴q轴强度和旋转的目标转子位置输出电机相线pwm占空比前文已经实现了SVPWM纯C语言代码这里将其放到单片机中也就是要将纯C的数学函数换做CMSIS-DSP里的函数
//foc.c
#include foc.h
#include arm_math.h
#include motor_runtime_param.h
#include stdbool.h
#define rad60 deg2rad(60)
#define SQRT3 1.73205080756887729353
#define deg2rad(a) (PI * (a) / 180)
#define rad2deg(a) (180 * (a) / PI)
#define max(a, b) ((a) (b) ? (a) : (b))
#define min(a, b) ((a) (b) ? (a) : (b))static void svpwm(float phi, float d, float q, float *d_u, float *d_v, float *d_w)
{d min(d, 1);d max(d, -1);q min(q, 1);q max(q, -1);const int v[6][3] {{1, 0, 0}, {1, 1, 0}, {0, 1, 0}, {0, 1, 1}, {0, 0, 1}, {1, 0, 1}};const int K_to_sector[] {4, 6, 5, 5, 3, 1, 2, 2};float sin_phi arm_sin_f32(phi);float cos_phi arm_cos_f32(phi);float alpha 0;float beta 0;arm_inv_park_f32(d, q, alpha, beta, sin_phi, cos_phi);bool A beta 0;bool B fabs(beta) SQRT3 * fabs(alpha);bool C alpha 0;int K 4 * A 2 * B C;int sector K_to_sector[K];float t_m arm_sin_f32(sector * rad60) * alpha - arm_cos_f32(sector * rad60) * beta;float t_n beta * arm_cos_f32(sector * rad60 - rad60) - alpha * arm_sin_f32(sector * rad60 - rad60);float t_0 1 - t_m - t_n;*d_u t_m * v[sector - 1][0] t_n * v[(sector) % 6][0] t_0 / 2;*d_v t_m * v[sector - 1][1] t_n * v[(sector) % 6][1] t_0 / 2;*d_w t_m * v[sector - 1][2] t_n * v[(sector) % 6][2] t_0 / 2;
}我将svpwm函数封装一层命名为foc_forward传入参数是d轴强度、q轴强度、转子角度后续闭环计算完毕后也能输入到该函数得到电机相线pwm占空比。这里创建了一个和set_pwm_duty同名的weak类型函数这样做是为了提高FOC代码的独立解耦性编译的时候如果有同名函数带weak的不会被编译进去。
//foc.c
//......
__attribute__((weak)) void set_pwm_duty(float d_u, float d_v, float d_w)
{while (1);
}
void foc_forward(float d, float q, float rotor_rad)
{float d_u 0;float d_v 0;float d_w 0;svpwm(rotor_rad, d, q, d_u, d_v, d_w);set_pwm_duty(d_u, d_v, d_w);
}
//......//foc.h
#pragma oncefloat cycle_diff(float diff, float cycle);
void foc_forward(float d, float q, float rotor_rad);测试一下FOC开环控制设定d轴强度为0、q轴强度为0.5不要太高开环发热、目标转子在0~360度之间循环转动此时电机应该就转起来了由于是开环电机转起来会抖而且会发热不要长时间转。
//main.c
//......
while(1)
{
//....../* USER CODE BEGIN 3 */for (size_t i 0; i 360; i 20){HAL_Delay(2);foc_forward(0, 0.5, deg2rad(i));}
//......确定电机正负旋转方向
默认情况下手动逆时针转动电机MT6701磁编码器的角度是递增的因此将逆时针旋转设定为角度增加的旋转正方向。有些编码器角度增加的旋转方向是逆时针这并不唯一。 继续使用上述FOC开环测试代码传入的q轴是正数如果电机逆时针转那么相线顺序是正确的否则将电机相线取两相对调即可得到正确的相线顺序。 得到正确的相线顺序后就可以认为UVW三相线分别对应PWM通道123。
电机旋转速度计算
众所周知 旋转速度 Δ θ Δ t Δ θ ∗ 频率 旋转速度\frac{\Delta \theta}{\Delta t}\Delta \theta*频率 旋转速度ΔtΔθΔθ∗频率速度计算频率我们已经设定了宏定义motor_speed_calc_freq所以在速度定时器中断里的计算过程是 ( 上次编码器角度 − 这次编码器角度 ) ∗ m o t o r _ s p e e d _ c a l c _ f r e q (上次编码器角度-这次编码器角度)*motor\_speed\_calc\_freq (上次编码器角度−这次编码器角度)∗motor_speed_calc_freq 但是代码不能直接这么写因为
编码器角度位于 0 0 0到 2 π 2\pi 2π之间359度到1度实际旋转了2度但是差值是358度因此计算差值的时候要把差值转换到 − π -\pi −π到 π \pi π之间。算出来的速度需要进行滤波才能给外部用。
下面这个函数能将无符号数映射到一个周期内的有符号数。算角度差值的时候经常会用到将其放在FOC代码中
//foc.c
//......
float cycle_diff(float diff, float cycle)
{if (diff (cycle / 2))diff - cycle;else if (diff (-cycle / 2))diff cycle;return diff;
}
//......//foc.h
#pragma once
//......
float cycle_diff(float diff, float cycle);
//......再实现滤波函数这里使用低通滤波也可以自行更换为卡尔曼滤波等等。 先创建文件用于放置滤波代码注意别忘了在keil里加入头文件路径和C文件这里keil里就不演示了。 再实现低通滤波函数
//filter.c
float low_pass_filter(float input, float last_output, float alpha)
{return alpha * input (1.0 - alpha) * last_output;
}在Src/tim.c实现最终的速度计算代码
//tim.c
#include global_def.h
#include motor/motor_runtime_param.h
#include motor/foc.h
#include algorithm/filter.h
#include arm_math.h
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{if (htim-Instance TIM3){static float encoder_angle_last 0;/******encoder_angle_last默认值是0不能用于计算需要赋初值*********/static int once 1;if (once){once !once;encoder_angle_last encoder_angle;}/***************/float diff_angle cycle_diff(encoder_angle - encoder_angle_last, 2 * PI);encoder_angle_last encoder_angle;float _motor_speed diff_angle * motor_speed_calc_freq;float filter_alpha_speed 0.1;//低通滤波参数如果希望保留更多的细节可以增加alpha的值motor_speed low_pass_filter(_motor_speed, motor_speed, filter_alpha_speed);}
}在main.c中开启速度计算定时器
//......
HAL_TIM_Base_Start_IT(htim3);
/* USER CODE END 2 */
//......测试一下速度计算是否有效继续使用FOC开环测试代码不要转太长时间开环控制会发热
//main.c
//......
while(1)
{
//....../* USER CODE BEGIN 3 */for (size_t i 0; i 360; i 20){HAL_Delay(2);foc_forward(0, 0.5, deg2rad(i));printf(%f\n, motor_speed);}
//......多圈逻辑角度
从MT6701编码器是 0 0 0到 2 π 2\pi 2π周期变化的只有单圈但我们想要多圈角度时需要自行将每次的角度差值累计起来即累计(当前角度-上次角度)。 逻辑角度是手动累计的因此更新及时性是取决与放在哪里累计的。如果放在SPI读取角度中断里那么和编码器角度一样的更新及时如果放在其他中断里比如速度计算定时器中断、ADC采样完成中断按照本文设置则逻辑角度更新速度等于速度计算频率930HzADC中断4KHz。更新及时性超过SPI读取角度中断频率是没有意义的。本文选择放在SPI读取角度中断里累计。 这里对逻辑角度进行了周期操作半周期是position_cycle比如按照度数表达如果半周期设置为360度、当前逻辑角度是358度再转3度后逻辑角度等于-359度。如果你的逻辑角度是非常非常多圈并且你自信电机实际旋转需求不会超出浮点数可表达的最大值可以不需要周期操作。 补充SPI读取角度中断
//spi.c
//......
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi)
{if (hspi-Instance SPI1){HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET);int angle_raw (mt6701_rx_data[1] 2) | (mt6701_rx_data[0] 6);encoder_angle 2 * 3.1415926 * angle_raw / ((1 14) - 1);static float encoder_angle_last 0;/****encoder_angle_last默认值是0不能用于计算要先赋值一次****/static int once 1;if (once){once !once;encoder_angle_last encoder_angle;}/*************/float _encoder_angle encoder_angle;// 角度差值用于累计多圈逻辑角度float diff_angle cycle_diff(_encoder_angle - encoder_angle_last, 2 * PI);encoder_angle_last _encoder_angle;// 实现周期操作将motor_logic_angle转到周期内motor_logic_angle cycle_diff(motor_logic_angle diff_angle, position_cycle);HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET);HAL_SPI_TransmitReceive_DMA(hspi1, mt6701_rx_data, mt6701_rx_data, 3);return;}
}
//......在主函数里打印一下逻辑角度用手转动电机几圈看看逻辑角度是否有效
//main.c
//......
while(1)
{
printf(%f\n, motor_logic_angle);
/* USER CODE END WHILE */
//......从角度结果可以看到逻辑角度具有多圈周期性
电流采样
电机相线上的电流采样电阻两端电压经过放大器放大后输入单片机ADC口。 本文会将正式的FOC实时计算代码放在ADC采样完成中断里大部分驱动代码都会这么做当然放在角度读取完成中断里也是可行的因此无论是否读取ADC值这个中断函数必须要开放实现的。 进行ADC采样之前先校准然后开始采样。由于ADC采样绑定了高级定时器的输出事件后续的ADC采样会自动被高级定时器触发。
//main.c
//......HAL_ADCEx_Calibration_Start(hadc1);HAL_ADCEx_Calibration_Start(hadc2);HAL_ADCEx_InjectedStart_IT(hadc1);HAL_ADCEx_InjectedStart(hadc2);/* USER CODE END 2 */
//......在ADC采样完成中断函数里读取ADC值INA199放大器系列的ADC值会在1.65V上下变动1.65V代表采样电阻两端电压为0V大于1.65V代表采样电阻两端电压为正数小于1.65V代表采样电阻两端电压为负数相线电流等于 A D C 值 − 1.65 V 放大倍数 ÷ 采样电阻值 \frac{ADC值-1.65V}{放大倍数}\div采样电阻值 放大倍数ADC值−1.65V÷采样电阻值。 这里最好验证一下ADC值的正负以及是否正好对应PWM通道123先假设PWM通道1对应ADC1PWM通道2对应ADC2则假设U相电流等于ADC1计算得到的电流V相电流等于ADC2计算得到的电流W相电流根据基尔霍夫电流总和为0的定律等于 − ( U 相电流 V 相电流 ) -(U相电流V相电流) −(U相电流V相电流)
//adc.c
//......
#include motor/motor_runtime_param.h
#include motor/foc.h
#include algorithm/filter.h
#include global_def.h
#include arm_math.h
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{if (hadc-Instance ADC1){float u_1 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc1, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float u_2 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc2, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float i_1 u_1 / R_SHUNT / OP_GAIN;float i_2 u_2 / R_SHUNT / OP_GAIN;motor_i_u i_1;motor_i_v i_2;}
}如果3个PWM通道正对应ADC1和ADC2那么打开PWM通道1关闭PWM通道2、3的时候ADC1这条路会得到一个对应的正电流值如下图所示
按照这个思想验证一下
//main.c
//....../* USER CODE BEGIN WHILE */set_pwm_duty(0.5, 0, 0);HAL_Delay(300);printf(%f,%f,%f\n, motor_i_u, motor_i_v, -(motor_i_u motor_i_v));set_pwm_duty(0, 0.5, 0);HAL_Delay(300);printf(%f,%f,%f\n, motor_i_u, motor_i_v, -(motor_i_u motor_i_v));set_pwm_duty(0, 0, 0.5);HAL_Delay(300);printf(%f,%f,%f\n, motor_i_u, motor_i_v, -(motor_i_u motor_i_v));set_pwm_duty(0, 0, 0);while (1){
//......如果得到类似下图的结果三个电流正值都差不多也是分别出现在第1、2、3个那么说明ADC1和ADC2正对应PWM通道1和2而且正负方向也是正确的否则就需要调整motor_i_u i_1;和motor_i_v i_2;代码重新排列正确的对应关系。 再根据3个相电流换算到d轴和q轴电流即clark变换和park变换理论部分请参考前文电流控制然后进行滤波操作。如果你不想使用力矩环d轴和q轴电流无需计算。ADC中断代码补充为
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{if (hadc-Instance ADC1){float u_1 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc1, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float u_2 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc2, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float i_1 u_1 / R_SHUNT / OP_GAIN;float i_2 u_2 / R_SHUNT / OP_GAIN;motor_i_u i_1;motor_i_v i_2;float i_alpha 0;float i_beta 0;arm_clarke_f32(motor_i_u, motor_i_v, i_alpha, i_beta);float sin_value arm_sin_f32(rotor_logic_angle);float cos_value arm_cos_f32(rotor_logic_angle);float _motor_i_d 0;float _motor_i_q 0;arm_park_f32(i_alpha, i_beta, _motor_i_d, _motor_i_q, sin_value, cos_value);float filter_alpha_i_d 0.1;float filter_alpha_i_q 0.1;motor_i_d low_pass_filter(_motor_i_d, motor_i_d, filter_alpha_i_d);motor_i_q low_pass_filter(_motor_i_q, motor_i_q, filter_alpha_i_q);}
}极对数
前文理论部分一直使用的是简单模型即转子看成一根直的磁铁实际电机的转子磁铁排布可能如下图这是本文使用的电机示意图一共有14个7对磁极即极对数为7。 极对数为7的电机完成7次相线换相周期后电机才会旋转一圈。也就是说用于FOC计算的转子逻辑角度极对数*转子物理角度。
转子角度确定
注意这里称为【转子】角度。 FOC模型是建立在电机转子基础上的d轴q轴相关计算是按照转子角度计算的而我们能直接获取的是磁编码器角度也就是电机外壳角度因为磁铁粘在电机外壳上磁编码器感应磁铁角度。 这就出现了一个问题转子零度与磁编码器零度之间差了多少度解决了这个问题那么转子角度就可知了即等于磁编码器角度减去这个偏差角度。 其中一个简单方法是电机上电后控制相线形成磁场将转子d轴吸引到转子零度的位置然后读取磁编码器数据这个角度就是偏差角度叫做d轴强拖。 用代码实现这个方法
//main.c
//......set_pwm_duty(0.5, 0, 0); // 生成SVPWM模型中的基础矢量1即对应转子零度位置HAL_Delay(400); // 保持一会转子吸引过来需要时间rotor_zero_angle motor_logic_angle; // 读取磁编码器数据刚上电用逻辑角度也可以set_pwm_duty(0, 0, 0); // 松开电机HAL_Delay(10);/* USER CODE END 2 */
//......前面在motor_runtime_param.h里设置的宏定义帮我们完成了转子角度的计算#define rotor_logic_angle rotor_phy_angle *POLE_PAIRS 闭环控制
这个部分编写位置环、速度环、电流环代码。FOC闭环控制代码将放在ADC采样中断中进行。
控制函数接口定义
控制函数接口要符合人的直觉让人可以直观地调用先定义函数接口再逐步补充这些函数
//foc.h
//......
void lib_position_control(float rad);
void lib_speed_control(float speed);
void lib_torque_control(float torque_norm_d, float torque_norm_q);
void lib_position_speed_torque_control(float position, float max_speed, float max_torque_norm);
//......先这么放着后面会补充内容
//foc.c
//......
void lib_position_control(float rad)
{
}void lib_speed_control(float speed)
{
}void lib_torque_control(float torque_norm_d, float torque_norm_q)
{
}void lib_speed_torque_control(float speed_rad, float max_torque_norm)
{
}void lib_position_speed_torque_control(float position_rad, float max_speed_rad, float max_torque_norm)
{
}
//......力矩控制包括d轴和q轴强度。 上述定义的接口意思是实时计算一次电机相线pwm输出占空比这是FOC核心算法因此需要不断调用上述接口以及时更新输出占空比。 为了让后续功能性的代码更加容易编写再对这个接口封装一层。这里创建了一个结构体变量用于方便后续程序上的使用你也可以不封装上述接口直接反复调用上述接口也是可以的。
//foc.h
//......
typedef enum
{control_type_null, // 不进行控制control_type_position, // 位置控制control_type_speed, // 速度控制control_type_torque, // 力矩控制control_type_position_speed_torque, // 位置-速度-力矩控制
} motor_control_type;typedef struct
{motor_control_type type;float position; // 目标角度单位radfloat speed; // 目标速度单位rad/sfloat torque_norm_d; // 目标d轴强度0~1float torque_norm_q; // 目标q轴强度0~1float max_speed; // 串级控制时的最大速度单位rad/sfloat max_torque_norm; // 串级控制时的最大q轴力矩0~1
} motor_control_context_t;extern motor_control_context_t motor_control_context;
//......有了这个封装后的结构体变量后就可以在ADC中断里不断扫描这个结构体变量从而不断调用对应的FOC控制函数接口在主代码里对这个结构体变量赋值即可。
//adc.c
//......
void HAL_ADCEx_InjectedConvCpltCallback(ADC_HandleTypeDef *hadc)
{if (hadc-Instance ADC1){float u_1 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc1, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float u_2 ADC_REFERENCE_VOLT * ((float)HAL_ADCEx_InjectedGetValue(hadc2, ADC_INJECTED_RANK_1) / ((1 ADC_BITS) - 1) - 0.5);float i_1 u_1 / R_SHUNT / OP_GAIN;float i_2 u_2 / R_SHUNT / OP_GAIN;motor_i_u i_1;motor_i_v i_2;float i_alpha 0;float i_beta 0;arm_clarke_f32(motor_i_u, motor_i_v, i_alpha, i_beta);float sin_value arm_sin_f32(rotor_logic_angle);float cos_value arm_cos_f32(rotor_logic_angle);float _motor_i_d 0;float _motor_i_q 0;arm_park_f32(i_alpha, i_beta, _motor_i_d, _motor_i_q, sin_value, cos_value);float filter_alpha_i_d 0.1;float filter_alpha_i_q 0.1;motor_i_d low_pass_filter(_motor_i_d, motor_i_d, filter_alpha_i_d);motor_i_q low_pass_filter(_motor_i_q, motor_i_q, filter_alpha_i_q);switch (motor_control_context.type){case control_type_position:lib_position_control(motor_control_context.position);break;case control_type_speed:lib_speed_control(motor_control_context.speed);break;case control_type_torque:lib_torque_control(motor_control_context.torque_norm_d, motor_control_context.torque_norm_q);break;case control_type_position_speed_torque:lib_position_speed_torque_control(motor_control_context.position, motor_control_context.max_speed, motor_control_context.max_torque_norm);break;default:break;}}
}
//......以位置控制为例目标位置是90度封装接口后的调用方式就变为
//main.c
//....../* USER CODE BEGIN WHILE */motor_control_context.position deg2rad(90);motor_control_context.type control_type_position;while (1){
//......CMSIS-DSP提供的PID控制器
CMSIS-DSP数学库中提供了一个PID控制器变量类型是arm_pid_instance_f32将其用在我们的闭环控制中。使用方法是先调用arm_pid_init_f32配置PID系数在闭环控制时使用arm_pid_f32计算输出PID输出值。该部分可以参考前文CMSIS-DSP。 设计一个函数给位置环、速度环、电流环分别创建pid控制器
//foc.c
//......
static arm_pid_instance_f32 pid_position;
static arm_pid_instance_f32 pid_speed;
static arm_pid_instance_f32 pid_torque_d;
static arm_pid_instance_f32 pid_torque_q;
void set_motor_pid(float position_p, float position_i, float position_d,float speed_p, float speed_i, float speed_d,float torque_d_p, float torque_d_i, float torque_d_d,float torque_q_p, float torque_q_i, float torque_q_d)
{pid_position.Kp position_p;pid_position.Ki position_i;pid_position.Kd position_d;pid_speed.Kp speed_p;pid_speed.Ki speed_i;pid_speed.Kd speed_d;pid_torque_d.Kp torque_d_p;pid_torque_d.Ki torque_d_i;pid_torque_d.Kd torque_d_d;pid_torque_q.Kp torque_q_p;pid_torque_q.Ki torque_q_i;pid_torque_q.Kd torque_q_d;arm_pid_init_f32(pid_position, false);//false代表清空内部增量数据arm_pid_init_f32(pid_speed, false);arm_pid_init_f32(pid_torque_d, false);arm_pid_init_f32(pid_torque_q, false);
}
//......//foc.h
//......
void set_motor_pid(float position_p, float position_i, float position_d,float speed_p, float speed_i, float speed_d,float torque_d_p, float torque_d_i, float torque_d_d,float torque_q_p, float torque_q_i, float torque_q_d);
//......在main.c中配置pid系数先设置为0后续调参过程中会反复修改这里的参数。
//main.c
//....../* USER CODE BEGIN 2 */set_motor_pid(0, 0, 0,0, 0, 0,0, 0, 0,0, 0, 0);
//......位置控制
位置环的角度差值注意要使用cycle_diff函数进行周期化因为角度是有周期的这里使用逻辑角度以便可以多圈控制。 差值到底是rad - motor_logic_angle还是motor_logic_angle - rad呢你可以这样想如果真实角度motor_logic_angle是100度输入目标角度rad是150度pid控制器需要输出正数才能让电机转向目标角度因此pid控制器也需要输入正数因此只有rad - motor_logic_angle才是得到正数。 position_loop函数是干嘛用的它有输入和输出可以和速度环、力矩环串起来因此单独隔离出一个函数方便控制环之间的串联。 d轴是不提供转向力矩的对位置环没有用因此目标d轴强度设置为0。 foc_forward开环控制在这里又用起来了这个函数的本质是将目标d轴q轴强度落实到电机相线pwm通道上。 后续的速度环和位置环同理。
//foc.c
//......
static float position_loop(float rad)
{float diff cycle_diff(rad - motor_logic_angle, position_cycle);return arm_pid_f32(pid_position, diff);
}void lib_position_control(float rad)
{float d 0;float q position_loop(rad);foc_forward(d, q, rotor_logic_angle);
}
//......速度控制
速度是没有周期的所以不需要使用cycle_diff函数周期化差值。
//foc.c
//......
static float speed_loop(float speed_rad)
{float diff speed_rad - motor_speed;return arm_pid_f32(pid_speed, diff);
}
void lib_speed_control(float speed)
{float d 0;float q speed_loop(speed);foc_forward(d, q, rotor_logic_angle);
}
//......力矩电流控制
d轴和q轴都有力矩环将d轴和q轴电流与宏定义设定的最大电流的比例看作【力矩强度】力矩环输入的是0到1之间的百分比强度数据。
//foc.c
//......
static float torque_d_loop(float d)
{float diff d - motor_i_d / MAX_CURRENT;return arm_pid_f32(pid_torque_d, diff);
}static float torque_q_loop(float q)
{float diff q - motor_i_q / MAX_CURRENT;return arm_pid_f32(pid_torque_q, diff);
}void lib_torque_control(float torque_norm_d, float torque_norm_q)
{float d torque_d_loop(torque_norm_d);float q torque_q_loop(torque_norm_q);foc_forward(d, q, rotor_logic_angle);
}
//......位置-速度-力矩控制
将位置环、速度环、力矩环串联起来就是【位置-速度-力矩控制】。可前往查看前文位置、速度、电流控制的控制框图。 位置-速度-力矩联合控制的时候主角是还是位置环速度、力矩是运行过程中的最大值比如输入的速度是30rad/s意思是电机角度到位过程中的速度按照30rad/s进行控制角度到位后电机速度会按照0rad/s进行控制。 这里的代码抽出了速度-力矩控制作为一个单独的函数这样既开放了【速度-力矩控制】的接口也可以被【位置-速度-力矩控制】调用。
//foc.c
//......
void lib_speed_torque_control(float speed_rad, float max_torque_norm)
{float torque_norm speed_loop(speed_rad);torque_norm min(fabs(torque_norm), max_torque_norm) * (torque_norm 0 ? 1 : -1);lib_torque_control(0, torque_norm);
}void lib_position_speed_torque_control(float position_rad, float max_speed_rad, float max_torque_norm)
{float speed_rad position_loop(position_rad);speed_rad min(fabs(speed_rad), max_speed_rad) * (speed_rad 0 ? 1 : -1);lib_speed_torque_control(speed_rad, max_torque_norm);
}
//......PID系数调节
到目前位置已经在ADC中断中放置了FOC代码也在main.c中设置好了闭环控制类型示例是位置控制只要再设置好PID系数电机就能进行闭环动作了。PID系数调节比较依靠经验由于本人能力有限我无法深入讲解网上有大量的PID系数调节教程。
位置环
本文选择的电机是云台电机这里的位置环的PID系数只用到P系数和D系数可以在主while(1)循环里打印电机实时角度辅助调节。 首先设定为位置控制模式由于FOC代码里位置控制是按照逻辑角度进行控制的因此这里设置的90度是逻辑角度
//main.c
//....../* USER CODE BEGIN WHILE */motor_control_context.position deg2rad(90);motor_control_context.type control_type_position;while (1){printf(%f\n, motor_logic_angle);/* USER CODE END WHILE */
//......调节P系数建议从2开始0.1为步距往上调整直到电机到位过程中出现轻微位置弹簧感 然后设定D系数建议从5开始1为步距往上调直到弹簧感消失这样一个归位迅速而回弹轻微的位置环就调节好了
速度环
调节转速环的时候转速滤波系数filter_alpha_speed非常重要决定了计算出来的速度是否平稳本文使用的低通滤波如果想要滤波平稳一点那么就会有滞后如果想要速度更新及时一点那么滤波结果波动就大这个转速滤波的参数也是需要自己调节测试的我为了验证FOC算法设定的0.07滞后比较厉害但是速度滤波出来比较平稳。 在主while(1)前设定FOC为速度控制模式
//main.c
//....../* USER CODE BEGIN WHILE */motor_control_context.speed 20 * 2 * PI;motor_control_context.type control_type_position;while (1){printf(%f\n, motor_speed);/* USER CODE END WHILE */
//......速度环主要是使用P系数和I系数速度波动比较剧烈D系数干扰大。P系数要给的非常小建议从0.01开始步距0.001往上调节I系数从0.001开始往上调节。 经过我简单的调节本文选用的电机在空载时在速度环模式下速度大概能到每秒122.6弧度每秒39转每分钟1171转左右。
力矩环
力矩环同样有滤波系数filter_alpha_i_d和filter_alpha_i_q分别对应d轴电流和q轴电流滤波我为了验证FOC算法这两个参数设置的0.1。 在主while(1)前设定FOC为力矩控制模式
//main.c
//....../* USER CODE BEGIN WHILE */motor_control_context.torque_norm_d 0;motor_control_context.torque_norm_q 1;motor_control_context.type control_type_torque;while (1){printf(%f\n, motor_speed);/* USER CODE END WHILE */
//......经过我简单的调节本文选用的电机在空载时在力矩环模式下速度大概能到每秒145.6弧度每秒23转每分钟1391转左右。
位置-速度-力矩串级PID
三者串级控制的时候不能直接使用单独控制时的PID系数需要重新调节要从最内环开始串级调节到最外环即先调好力矩控制再调好速度-力矩控制再调好位置-速度-力矩控制。 注意点
注意调试过程中打断点或者点击Stop Debug的时候不要暂停时间过长因为暂停的时候电机相线的pwm占空比也固定住了如果某一相的占空比刚好固定在非常高的状态会导致电机迅速发热。stm32f103系列的算力有限keil编译的时候请开启-O3优化否则可能需要再降低SPI频率或者提高高级定时器重复计数器以降低FOC代码计算频率。 至此已经完成了从零开始实现stm32无刷电机FOC本文源码开源可前往查看https://gitee.com/best_pureer/stm32_foc。
