#include “encoder.h”
#include “main.h”
#include “usart.h”
#include “stdio.h”
#include “tim.h”
#include “math.h”
#include “hall.h”
#include “mymath.h”
#include “gpio.h”
#include “arm_math.h”
#include “adc.h”
#include “foc.h”
float Encoder_Theta_coefficient; //编码器转换成电角度的系数值
float Encoder_Theta_m_coefficient; //编码器转换成机械角度的系数值
float We_coefficient; //编码器转电角速度的系数值
float Wm_coefficient; //编码器转机械角速度的系数值
float t;
float Current_BandWidth; //电流环带宽
float CurrentLoop_Kpd; //电流环D轴比例增益
float CurrentLoop_Kid ; //电流环D轴积分增益
float CurrentLoop_Kpq ; //电流环Q轴比例增益
float CurrentLoop_Kiq; //电流环Q轴积分增益
int Speed_BandWidth ; //速度环带宽
float Ba ; //有功阻尼系数
float SpeedLoop_Kp ; //速度环比例增益
float SpeedLoop_Ki ; //速度环积分增益
void Foc_Init()
{
float temp1,temp2;
Motor1.Wm_Set=0; Motor1.Id_Set=0; //设定为20k（电流采样由定时器1的PWM比较触发，频率也为20k） Motor1_Time.PWM_Fre = 168000000.0f / (TIM1_ARR * TIM1_PSC) / 2; Motor1_Time.PWM_Ts = 1 / Motor1_Time.PWM_Fre; //电流环：速度环：位置环的速度比例有多种，有16：4：1， 2：1：1， 1:1:1,都可以 Motor1_Time.Current_Loop_TimeGain = 1.0f; Motor1_Time.Speed_Loop_TimeGain = 1.0f; Motor1_Time.Locate_Loop_TimeGain = 1.0f; //计算频率设定为40k Motor1_Time.Calculate_Fre = 84000000.0f / (TIM2_ARR * TIM2_PSC ); Motor1_Time.Calculate_Ts = 1 / Motor1_Time.Calculate_Fre; //电流环设定为20k Motor1_Time.Current_Loop_Fre = Motor1_Time.Calculate_Fre / Motor1_Time.Current_Loop_TimeGain; Motor1_Time.Current_Loop_Ts = 1 / Motor1_Time.Current_Loop_Fre; //速度环设定为10k Motor1_Time.Speed_Loop_Fre = Motor1_Time.Calculate_Fre / Motor1_Time.Speed_Loop_TimeGain; Motor1_Time.Speed_Loop_Ts = 1 / Motor1_Time.Speed_Loop_Fre; //位置设定为10k Motor1_Time.Locate_Loop_Fre = Motor1_Time.Calculate_Fre / Motor1_Time.Locate_Loop_TimeGain; Motor1_Time.Locate_Loop_Ts = 1 / Motor1_Time.Locate_Loop_Fre; //编码器计数值转换为角度的系数值 Encoder_Theta_coefficient = 360 * MOTOR1_Pn /(ENCODER_NUMBER * 4); Encoder_Theta_m_coefficient = 360 /(ENCODER_NUMBER * 4); //编码器计数值转换为角速度的系数值 We_coefficient = 2* PI /(ENCODER_NUMBER * 4) / Motor1_Time.Speed_Loop_Ts; Wm_coefficient = 60.0f /(ENCODER_NUMBER * 4) / Motor1_Time.Speed_Loop_Ts; //计算电流环的PI系数 temp1=MOTOR1_Ld/MOTOR1_R; temp2=MOTOR1_Lq/MOTOR1_R; t=(temp1<temp2)?temp1:temp2; Current_BandWidth = 2*PI/t; //算出来的带宽太大了，有九千多，感觉是电机的参数给的可能有问题？不知道，反正不用这个带宽 //小了响应速度不够快，大了电流波动大 Current_BandWidth =1500; CurrentLoop_Kpd = Current_BandWidth * MOTOR1_Ld; CurrentLoop_Kid = Current_BandWidth * MOTOR1_R; CurrentLoop_Kpq = Current_BandWidth * MOTOR1_Ld; CurrentLoop_Kiq = Current_BandWidth * MOTOR1_R; //计算速度环的带宽 Speed_BandWidth = 150; Ba = (Speed_BandWidth *MOTOR1_J - MOTOR1_B ) / (1.5f * MOTOR1_Pn * MOTOR1_flux); //这个值在速度环我看别人的仿真不知道为啥给扔了，在速度环的时候不要这个 SpeedLoop_Kp = ( Speed_BandWidth * MOTOR1_J ) / (1.5f * MOTOR1_Pn * MOTOR1_flux) ; SpeedLoop_Ki = Speed_BandWidth * SpeedLoop_Kp;
}
//整个闭环控制计算可以满足40k的处理频率
void Foc_Control(void)
{
static int Number1=0,Number2=0,Number3=0;
Motor1.Encoder_Value=Get_Encoder_Count(); //根据编码器计算角度 Motor1.Theta=Value_Limit(( Motor1.Encoder_Value + ENCODER_OFFSET ) * Encoder_Theta_coefficient,0.0f,360.0f); Motor1.Theta_m = Value_Limit(( Motor1.Encoder_Value + ENCODER_OFFSET ) * Encoder_Theta_m_coefficient,0.0f,360.0f); if(++Number1 >= Motor1_Time.Locate_Loop_TimeGain) { //执行位置环 Locate_Loop(); Number1= 0; } if(++Number2 >= Motor1_Time.Speed_Loop_TimeGain) { //执行速度环 Speed_Loop(); Number2= 0; } if(++Number3 >= Motor1_Time.Current_Loop_TimeGain) { //执行电流环 Current_Loop(); Number3= 0; } //对电压进行反Park变换 Park_Inverse_Transform(); //执行SVPWM并设定电机占空比 FOC_SVPWM(); //执行高频方波注入算法
// HFI_Sensorless();
}
//SVPWM
void FOC_SVPWM(void)
{
uint8_t N,A,B,C;
float Vref1,Vref2,Vref3,X,Y,Z,temp1,Tfirst,Tsecond,T0,Ta,Tb,Tc,Tcm1,Tcm2,Tcm3;
//计算转子所在的山区 Vref1=Motor1.Ubeta; Vref2=(SQRT_3* Motor1.Ualpha- Motor1.Ubeta)/2; Vref3=(-SQRT_3* Motor1.Ualpha- Motor1.Ubeta)/2; A=Vref1>0 ? 1 :0 ; B=Vref2>0 ? 1 :0 ; C=Vref3>0 ? 1 :0 ; N=4*C+2*B+A; temp1=SQRT_3* SVPWM_TS/ UDC; X=temp1*Vref1; Y=-temp1*Vref3; Z=-temp1*Vref2; //矢量作用时间计算 switch(N) { case 1: Tfirst= Z; Tsecond= Y; Motor1.Sector= 2; break; case 2: Tfirst= Y; Tsecond= -X; Motor1.Sector= 6; break; case 3: Tfirst= -Z; Tsecond= X; Motor1.Sector= 1; break; case 4: Tfirst= -X; Tsecond= Z; Motor1.Sector= 4; break; case 5: Tfirst= X; Tsecond= -Y; Motor1.Sector= 3; break; case 6: Tfirst= -Y; Tsecond= -Z; Motor1.Sector= 5; break; default: Tfirst= 0; Tsecond= 0; Motor1.Sector= 0; break; } //超限判断 if(( Tfirst + Tsecond )> SVPWM_TS) { Tfirst=(Tfirst/(Tfirst+Tsecond))/ SVPWM_TS; Tsecond=(Tsecond/(Tfirst+Tsecond))/ SVPWM_TS; } T0= (SVPWM_TS- Tfirst- Tsecond)/2; Ta=T0/2; Tb=Ta+Tfirst/2; Tc=Tb+Tsecond/2; //每相桥臂切换时间计算 switch(N) { case 1: Tcm1=Tb; Tcm2=Ta; Tcm3=Tc; break; case 2: Tcm1=Ta; Tcm2=Tc; Tcm3=Tb; break; case 3: Tcm1=Ta; Tcm2=Tb; Tcm3=Tc; break; case 4: Tcm1=Tc; Tcm2=Tb; Tcm3=Ta; break; case 5: Tcm1=Tc; Tcm2=Ta; Tcm3=Tb; break; case 6: Tcm1=Tb; Tcm2=Tc; Tcm3=Ta; break; default: break; } Motor1.Tcm1=Tcm1; Motor1.Tcm2=Tcm2; Motor1.Tcm3=Tcm3; //设置定时器1的PWM占空比 __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_1,TIM1_ARR*(1-2*Tcm1/ SVPWM_TS)); __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_2,TIM1_ARR*(1-2*Tcm2/ SVPWM_TS)); __HAL_TIM_SET_COMPARE(&htim1,TIM_CHANNEL_3,TIM1_ARR*(1-2*Tcm3/ SVPWM_TS));
}
void ADC_Transform(void)
{
//顺便软件滤波一下
//按照无刷驱动板的原理图上的公式算的，0.02是电阻值，电流转成的电压 除以电阻就是电流
// Motor1.Ibeta=((HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1) * 3.3f /4095.0f) -1.24f)/8.0f/0.02f; 单位A
Motor1.Ia =IRFilter1(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_1)) * 0.005f -7.75f;
Motor1.Ib =IRFilter2(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_2)) * 0.005f -7.75f;
Motor1.Ic =IRFilter3(HAL_ADCEx_InjectedGetValue(&hadc1,ADC_INJECTED_RANK_3)) * 0.005f -7.75f;
}
//位置环
void Locate_Loop(void)
{
//用不上暂时还没学，就没写
}
//速度环
void Speed_Loop(void)
{
static float temp;
static int Encoder_Last;
float Encoder_W_temp;
if(Encoder_Z_Flag==1) { Encoder_W_temp = KalmanFilter(Motor1.Encoder_Value - Encoder_Last + Encoder_temp); Encoder_Z_Flag=0; } else Encoder_W_temp = KalmanFilter(Motor1.Encoder_Value - Encoder_Last); //根据编码器计算角速度 速度范围波动会比较大，因为采样频率太高了，编码器的分辨率又不够高，所以这是很正常的。降低采样频率（即减小TIM2的时钟频率）可以减小波动 Motor1.We = Encoder_W_temp * We_coefficient; //单位 rad/s(电角速度) Motor1.Wm = Encoder_W_temp * Wm_coefficient; //单位 RPM(角速度) Encoder_Last=Motor1.Encoder_Value; Motor1.Iq_Set = SpeedLoop_Kp * (Motor1.Wm_Set - Motor1.Wm) + temp; temp = Value_SetMaxMin(temp +SpeedLoop_Ki * (Motor1.Wm_Set - Motor1.Wm) * Motor1_Time.Speed_Loop_Ts,-10.0f,10.0f); //给个限值防止他一直往上加，一启动电机速度飞飚，这个值是慢慢试的，大概速度到3000RPM时，这个值为10.多
}
//电流环
void Current_Loop(void)
{
static float temp,temp2;
//对三相电流进行采样，三相电流相加不为0，是因为硬件上有误差（野火客服说的，可恶！） 电流采样频率和电流频率一样都为20k ADC_Transform(); //对电流进行Clark变换，再Park变换 Motor1.Ialpha = Motor1.Ia; Motor1.Ibeta = (Motor1.Ia + 2*Motor1.Ib)/SQRT_3; Motor1.Id = Motor1.Ialpha* qfcosd(Motor1.Theta)+ Motor1.Ibeta* qfsind(Motor1.Theta); Motor1.Iq = -Motor1.Ialpha* qfsind(Motor1.Theta)+ Motor1.Ibeta* qfcosd(Motor1.Theta); //PI环节 //这个电流环的积分项我没整明白，加上积分的话，积分项会直接冲到最大，且一直为最大，然后就电机转动也没有更好，感觉就是反作用，先不整了，把它干掉，后面再研究 Motor1.Ud = CurrentLoop_Kpd *(Motor1.Id_Set - Motor1.Id) + temp - Motor1.We * MOTOR1_Lq * Motor1.Iq;
// temp = Value_SetMaxMin( temp+ CurrentLoop_Kid * (Motor1.Id_Set - Motor1.Id) * Motor1.Calculate_Ts, -0.1f, 0.1f);
Motor1.Uq = CurrentLoop_Kpq *(Motor1.Iq_Set - Motor1.Iq) + temp2 + Motor1.We * (MOTOR1_Ld * Motor1.Id + MOTOR1_flux);
// temp2 = Value_SetMaxMin( temp2+ CurrentLoop_Kiq * (Motor1.Iq_Set - Motor1.Iq) * Motor1.Calculate_Ts, -0.1f, 0.1f);
//合成电压最大值为SQRT_3 * UDC / 3 = 13.85 , Ud 最大值设为3 是为了高频方波注入算法， 这些限值都是试出来的，我也不会算理论值；这样限值转速最大能到2600RPM左右，够了 Motor1.Ud = Value_SetMaxMin(Motor1.Ud,-3.0f,3.0f); Motor1.Uq = Value_SetMaxMin(Motor1.Uq,-12.0f,12.0f);
// Motor1.Ud = 0;
// Motor1.Uq = 2;
}
这是电机控制FOC的速度和电流环驱动代码
#include "main.h"
#include "adc.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "bsp_key.h"
#include "hall.h"
#include "arm_math.h"
#include "arm_const_structs.h"
#include "math.h"
#include "encoder.h"
#include "mymath.h"
#include "foc.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
MOTOR Motor1;
TIME Motor1_Time;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_TIM1_Init();
MX_TIM2_Init();
MX_TIM5_Init();
MX_USART1_UART_Init();
MX_ADC1_Init();
MX_DMA_Init();
/* USER CODE BEGIN 2 */
My_Timer_Init();
Encoder_Init();
Foc_Init();
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
//按键1按下，电机启动
if(Key_Scan(KEY1_GPIO_Port,KEY1_Pin)==KEY_ON)
{
//点亮LED_D11,串口输出电机启动，打开控制端口，设置初始速度（占空比）和初始方向，判断扇区并启动一次
LED_D11_ON;
printf("Motor Running\r\n");
HAL_GPIO_WritePin(MOTOR_SD_GPIO_Port,MOTOR_SD_Pin,GPIO_PIN_SET);
// Motor1.Direction=Motor_Clockwise;
// Motor1.Speed=Motor_Speed_20; //初始执行一次霍尔换相
// HAL_TIM_PeriodElapsedCallback(&htim2);
Motor1.Wm_Set=800;
}else if(Key_Scan(KEY2_GPIO_Port,KEY2_Pin)==KEY_ON) //按键2按下，电机停止
{
//关闭LED_D11,串口输出电机停止，关闭控制端口，设置初始速度（占空比）和初始方向
LED_D11_OFF;
printf("Motor Stop\r\n");
HAL_GPIO_WritePin(MOTOR_SD_GPIO_Port,MOTOR_SD_Pin,GPIO_PIN_RESET);
// Motor1.Direction=Motor_Stop;
// Motor1.Speed=Motor_Speed_0;
Motor1.Wm_Set=0;
}else if(Key_Scan(KEY3_GPIO_Port,KEY3_Pin)==KEY_ON) //按键3按下，电机加速
{
// if(Motor1.Speed<Motor_Speed_100)
// Motor1.Speed++;
Motor1.Wm_Set +=100;
}else if(Key_Scan(KEY4_GPIO_Port,KEY4_Pin)==KEY_ON) //按键4按下，电机减速
{
// if(Motor1.Speed>Motor_Speed_20)
// Motor1.Speed--;
Motor1.Wm_Set -=100;
}else if(Key_Scan(KEY5_GPIO_Port,KEY5_Pin)==KEY_ON) //按键5按下，电机反向
{
// if(Motor1.Direction==Motor_Clockwise)
// Motor1.Direction=Motor_AntiClockwise;
// else if(Motor1.Direction==Motor_AntiClockwise)
// Motor1.Direction=Motor_Clockwise;
}
printf("%.2f,%.2f\n",Motor1.Wm,Motor1.Wm_Set);
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
RCC_OscInitStruct.PLL.PLLM = 8;
RCC_OscInitStruct.PLL.PLLN = 168;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
//定时器2更新中断（20k）
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if(htim == &htim2)
{
//测试中断所需时间
HAL_GPIO_WritePin(GPIO_TEST1_GPIO_Port,GPIO_TEST1_Pin,GPIO_PIN_SET);
Foc_Control();
HAL_GPIO_WritePin(GPIO_TEST1_GPIO_Port,GPIO_TEST1_Pin,GPIO_PIN_RESET);
}
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
这是主函数，我现在需要你为代码添加位置环的控制部分，要求可以实现位置控制的功能
最新发布
扫一扫
2317
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
