基于STM32F103C8T6的同步电机驱动-ADC采样与基于MT6701的角度获取
本系列文章：
- 基于STM32F103C8T6的同步电机驱动-CubeMX配置与IQmath调用
- 基于STM32F103C8T6的同步电机驱动-PWM驱动代码以及SVPWM的实现
- 基于STM32F103C8T6的同步电机驱动-ADC采样与基于MT6701的角度获取
一、ADC采样与电流电压计算相关代码编写
头文件、宏定义、变量定义等
/****file:main.c*****/
//Include Header Files
#include "main.h"
#include "adc.h"
#include "can.h"
#include "i2c.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
#include "stdio.h"
#include "IQmathlib.h"
//常用数学值定义
#define pi 3.14159f //pi
#define pi_2 6.28318f //2*pi
#define pi_23 2.094f //2*pi/3
#define pi_180 0.01745f //2*pi/360
#define sqrt3 1.73205f //sqrt(3)
#define sqrt3_2 0.86603f //sqrt(3)/2
#define sqrt23 0.81649f //sqrt(2/3)
#define sqrt24 0.70710f //sqrt(2/4)
//ADC相关参数（选用CC6903电流采样芯片）
uint16_t ADC_Value[4]={0}; //存储ADC采样值，范围为0-4095
#define K_1 0.00081f //ADC电流计算中：3.3/4096
#define K_2 7.57576f //132mv/A:1000/132
#define K_3 0.132f //1A/132mv:132/1000
//电流相关参数
_iq i_a ;_iq i_b ;_iq i_c ; //三相电流值
_iq i_alpha ; _iq i_beta ; //两相静止坐标电流值
_iq i_d ; _iq i_q ; //两相旋转坐标电流值
_iq i_d_lpf ; _iq i_q_lpf ; //两相旋转坐标电流值
//电压相关参数
_iq u_a ;_iq u_b ;_iq u_c ; //三相电压值
_iq u_alpha ; _iq u_beta ; //两相静止坐标电压值
_iq u_d ; _iq u_q ; //两相旋转坐标电压值
_iq u_dc_now;
_iq u_ref;
//角度相关参数
int32_t now_angle=0; //SPI获取的编码器原值，now_theta = now_angle*360/16384
int32_t now_angle_offcet=377;//编码器原值与电子角度0度之间的偏移值
_iq now_theta_M ; //当前机械角度值，范围为0-360°
_iq last_theta_M ; //上一周期机械角度值，范围为0-360°
_iq now_theta_E ; //当前电子角度值，范围为0-360°
_iq now_radian; //当前弧度值，范围为0-2*pi，now_radian = now_theta *2pi/360 ；IQ格式的三角函数计算需要弧度值。
TIM4中断回调函数，即驱动控制算法运行函数
//TIM4的中断回调函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim == &htim4)
{
//中断进来先改变占空比，此时占空比为上一周期计算得到的值
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, PWM1_PULSE);
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, PWM2_PULSE);
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, PWM3_PULSE);
//在此后面加入电机驱动相关代码
//电流电压ADC采样与计算
//获取四路ADC采样值，其中 AD0 = ADC_Value[1] ；AD1 = ADC_Value[2] ；AD2 = ADC_Value[3] ；AD3 = ADC_Value[0]
get_ADCvalue();
//计算相电流、母线电压、母线电流值
i_a = _IQmpy(_IQmpy(_IQ(ADC_Value[2]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2)) ;
i_b = _IQmpy(_IQmpy(_IQ(ADC_Value[3]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2)) ;
i_c = -i_a - i_b;
i_dc_now = _IQmpy(_IQmpy(_IQ(ADC_Value[0]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2));
u_dc_now = _IQmpy( _IQ(ADC_Value[1]) , _IQ(0.00793)) ;
//CLARK、PARK变换
i_alpha = _IQmpy((i_a - _IQmpy(_IQ(0.5), i_b) - _IQmpy(_IQ(0.5), i_c)), _IQ(K_park));
i_beta = _IQmpy((_IQmpy(_IQ(sqrt3_2), i_b) - _IQmpy(_IQ(sqrt3_2), i_c)), _IQ(K_park));
i_d = _IQmpy( _IQcos(now_radian) , i_alpha ) +_IQmpy( _IQsin(now_radian) , i_beta );
i_q = _IQmpy( -_IQsin(now_radian) , i_alpha ) +_IQmpy( _IQcos(now_radian) , i_beta );
//编码器获取角度与速度
//转速PI控制器
//电流PI控制器
//SVPWM
U1 = u_beta;
U2 = _IQmpy( _IQ(sqrt3_2) , u_alpha ) - _IQmpy(_IQ(0.5) , u_beta);
U3 = _IQmpy( _IQ(-sqrt3_2) , u_alpha ) - _IQmpy(_IQ(0.5) , u_beta);
if(U1>0){A=1;} else{A=0;}
if(U2>0){B=1;} else{B=0;}
if(U3>0){C=1;} else{C=0;}
N=A+2*B+4*C;
//对应扇区 //算T时不乘以Ts 算占空比时不除Ts,相互抵消
switch (N)
{
case 3://第一扇区 0-4-6-7-6-4-0
T4 = _IQmpy( _IQ(K) ,U2 );
T6 = _IQmpy( _IQ(K) ,U1 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T4-T6 );
T7 = T0;
T1 = _IQ(0);T2 =_IQ(0);T3 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T4 + T6 + T7;
duty_ratio2 = T6 + T7;
duty_ratio3 = T7;
break;
case 1://第二扇区 0-2-6-7-6-2-0
T2 = _IQmpy( _IQ(-K) ,U2 );
T6 = _IQmpy( _IQ(-K) ,U3 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T2-T6 );
T7 = T0;
T1 = _IQ(0);T3 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T6 + T7;
duty_ratio2 = T2 + T6 + T7;
duty_ratio3 = T7;
break;
case 5://第三扇区 0-2-3-7-3-2-0
T2 = _IQmpy( _IQ(K) ,U1 );
T3 = _IQmpy( _IQ(K) ,U3 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T2-T3 );
T7 = T0;
T1 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T7;
duty_ratio2 = T2 + T3 + T7;
duty_ratio3 = T3 + T7;
break;
case 4://第四扇区 0-1-3-7-3-2-0
T1 = _IQmpy( _IQ(-K) ,U1 );
T3 = _IQmpy( _IQ(-K) ,U2 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T1-T3 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T7;
duty_ratio2 = T3 + T7;
duty_ratio3 = T1 + T3 + T7;
break;
case 6://第五扇区 0-1-5-7-5-1-0
T1 = _IQmpy( _IQ(K) ,U3 );
T5 = _IQmpy( _IQ(K) ,U2 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T1-T5 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T3 =_IQ(0);
duty_ratio1 = T5 + T7;
duty_ratio2 = T7;
duty_ratio3 = T1 + T5 + T7;
break;
case 2://第六扇区 0-4-5-7-5-4-0
T4 = _IQmpy( _IQ(-K) ,U3 );
T5 = _IQmpy( _IQ(-K) ,U1 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T4-T5 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T1 =_IQ(0);T3 =_IQ(0);
duty_ratio1 = T4 + T5 + T7;
duty_ratio2 = T7;
duty_ratio3 = T5 + T7;
break;
default:
break;
}
//计算占空比值
PWM1_PULSE = 400*_IQtoF(duty_ratio1);
PWM2_PULSE = 400*_IQtoF(duty_ratio2);
PWM3_PULSE = 400*_IQtoF(duty_ratio3);
//串口信息打印
}
}
//其中函数get_ADCvalue();
void get_ADCvalue(void)
{
for(int i=0;i<4;i++)
{
HAL_ADC_Start(&hadc1);
ADC_Value[i]= HAL_ADC_GetValue(&hadc1);
}
}
二、ADC采样与电流电压计算相关代码编写
TIM4中断回调函数，即驱动控制算法运行函数
//TIM4的中断回调函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim == &htim4)
{
//中断进来先改变占空比，此时占空比为上一周期计算得到的值
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_1, PWM1_PULSE);
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_2, PWM2_PULSE);
__HAL_TIM_SetCompare(&htim1, TIM_CHANNEL_3, PWM3_PULSE);
//在此后面加入电机驱动相关代码
//电流电压ADC采样与计算
//获取四路ADC采样值，其中 AD0 = ADC_Value[1] ；AD1 = ADC_Value[2] ；AD2 = ADC_Value[3] ；AD3 = ADC_Value[0]
get_ADCvalue();
//计算相电流、母线电压、母线电流值
i_a = _IQmpy(_IQmpy(_IQ(ADC_Value[2]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2)) ;
i_b = _IQmpy(_IQmpy(_IQ(ADC_Value[3]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2)) ;
i_c = -i_a - i_b;
i_dc_now = _IQmpy(_IQmpy(_IQ(ADC_Value[0]), _IQ(K_1)) - _IQ(1.65), _IQ(K_2));
u_dc_now = _IQmpy( _IQ(ADC_Value[1]) , _IQ(0.00793)) ;
//CLARK、PARK变换
i_alpha = _IQmpy((i_a - _IQmpy(_IQ(0.5), i_b) - _IQmpy(_IQ(0.5), i_c)), _IQ(K_park));
i_beta = _IQmpy((_IQmpy(_IQ(sqrt3_2), i_b) - _IQmpy(_IQ(sqrt3_2), i_c)), _IQ(K_park));
i_d = _IQmpy( _IQcos(now_radian) , i_alpha ) +_IQmpy( _IQsin(now_radian) , i_beta );
i_q = _IQmpy( -_IQsin(now_radian) , i_alpha ) +_IQmpy( _IQcos(now_radian) , i_beta );
//编码器获取角度与速度
//获取当前编码器角度值，其并非实际角度值，实际角度值 = now_angle*360/16384
now_angle = spi_get_angle() - now_angle_offcet;
if (now_angle <0) now_angle = now_angle + 16384;
//获取当前角度值，now_theta为机械角度0-360°；now_theta_E为电子角度0-360°；now_radian为电子弧度0-2*pi
now_theta_M = _IQmpy( _IQ(now_angle) , _IQ(0.02197266) );
now_theta_E = _IQfrac(_IQmpy( now_theta_M , _IQ(0.01944444) ));
now_theta_E = _IQmpy( now_theta_E, _IQ(360) );
now_radian = _IQmpy( now_theta_E , _IQ(pi_180) );
//获取当前转速
//转速PI控制器
//电流PI控制器
//SVPWM
U1 = u_beta;
U2 = _IQmpy( _IQ(sqrt3_2) , u_alpha ) - _IQmpy(_IQ(0.5) , u_beta);
U3 = _IQmpy( _IQ(-sqrt3_2) , u_alpha ) - _IQmpy(_IQ(0.5) , u_beta);
if(U1>0){A=1;} else{A=0;}
if(U2>0){B=1;} else{B=0;}
if(U3>0){C=1;} else{C=0;}
N=A+2*B+4*C;
//对应扇区 //算T时不乘以Ts 算占空比时不除Ts,相互抵消
switch (N)
{
case 3://第一扇区 0-4-6-7-6-4-0
T4 = _IQmpy( _IQ(K) ,U2 );
T6 = _IQmpy( _IQ(K) ,U1 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T4-T6 );
T7 = T0;
T1 = _IQ(0);T2 =_IQ(0);T3 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T4 + T6 + T7;
duty_ratio2 = T6 + T7;
duty_ratio3 = T7;
break;
case 1://第二扇区 0-2-6-7-6-2-0
T2 = _IQmpy( _IQ(-K) ,U2 );
T6 = _IQmpy( _IQ(-K) ,U3 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T2-T6 );
T7 = T0;
T1 = _IQ(0);T3 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T6 + T7;
duty_ratio2 = T2 + T6 + T7;
duty_ratio3 = T7;
break;
case 5://第三扇区 0-2-3-7-3-2-0
T2 = _IQmpy( _IQ(K) ,U1 );
T3 = _IQmpy( _IQ(K) ,U3 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T2-T3 );
T7 = T0;
T1 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T7;
duty_ratio2 = T2 + T3 + T7;
duty_ratio3 = T3 + T7;
break;
case 4://第四扇区 0-1-3-7-3-2-0
T1 = _IQmpy( _IQ(-K) ,U1 );
T3 = _IQmpy( _IQ(-K) ,U2 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T1-T3 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T5 =_IQ(0);
duty_ratio1 = T7;
duty_ratio2 = T3 + T7;
duty_ratio3 = T1 + T3 + T7;
break;
case 6://第五扇区 0-1-5-7-5-1-0
T1 = _IQmpy( _IQ(K) ,U3 );
T5 = _IQmpy( _IQ(K) ,U2 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T1-T5 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T4 =_IQ(0);T3 =_IQ(0);
duty_ratio1 = T5 + T7;
duty_ratio2 = T7;
duty_ratio3 = T1 + T5 + T7;
break;
case 2://第六扇区 0-4-5-7-5-4-0
T4 = _IQmpy( _IQ(-K) ,U3 );
T5 = _IQmpy( _IQ(-K) ,U1 );
T0 = _IQmpy( _IQ(0.5) , _IQ(1.0)-T4-T5 );
T7 = T0;
T2 = _IQ(0);T6 =_IQ(0);T1 =_IQ(0);T3 =_IQ(0);
duty_ratio1 = T4 + T5 + T7;
duty_ratio2 = T7;
duty_ratio3 = T5 + T7;
break;
default:
break;
}
//计算占空比值
PWM1_PULSE = 400*_IQtoF(duty_ratio1);
PWM2_PULSE = 400*_IQtoF(duty_ratio2);
PWM3_PULSE = 400*_IQtoF(duty_ratio3);
//串口信息打印
}
}
//其中函数spi_get_angle();
uint32_t spi_get_angle(void)
{
uint8_t SPI_TXDATA[3];
uint8_t SPI_RXDATA[3];
uint32_t angle;
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15,GPIO_PIN_RESET);
HAL_SPI_TransmitReceive(&hspi1,SPI_TXDATA,SPI_RXDATA,3,0xffff);
HAL_GPIO_WritePin(GPIOA, GPIO_PIN_15,GPIO_PIN_SET);
angle = ((SPI_RXDATA[0]<<16)|(SPI_RXDATA[1]<<8)|SPI_RXDATA[2])>>10;
return angle;
}