#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****/ 这是主函数,我现在需要你为代码添加位置环的控制部分,要求可以实现位置控制的功能
时间: 2025-07-20 21:21:35 浏览: 8
<think>我们将在现有代码的基础上添加位置环控制。根据之前的讨论,我们需要在FOC控制循环中集成位置环,并且位置环的执行频率由Motor1_Time.Locate_Loop_TimeGain控制(在Foc_Init中初始化为1,即与计算频率相同,40kHz)。但实际位置环不需要这么高的频率,我们可以调整Motor1_Time.Locate_Loop_TimeGain来降低位置环的执行频率。
位置环控制思路:
1. 添加目标位置变量(target_angle)和位置控制使能标志(position_enabled)到Motor1结构体中,或者作为全局变量。
2. 在位置环函数Locate_Loop()中,如果位置控制使能,则计算位置误差(注意角度是0-360°的循环,需要处理边界),然后通过位置控制器(例如比例控制器)计算出速度指令(Motor1.Wm_Set)。
3. 在速度环中,速度环的设定值Motor1.Wm_Set将由位置环的输出提供(当位置控制使能时)。
修改步骤:
步骤1:在foc.h中定义位置环相关的变量和函数
步骤2:在foc.c中实现位置环函数Locate_Loop()
步骤3:修改main.c,添加按键控制目标位置和位置控制使能
考虑到现有代码结构,我们将在Motor1结构体中添加位置环相关的变量。但是,由于Motor1结构体在foc.h中定义,我们可以在foc.h中修改Motor1结构体。
但是,由于现有代码中Motor1结构体没有位置环相关变量,我们需要扩展它。另外,为了避免对原有代码造成太大影响,我们也可以使用全局变量。这里为了保持代码的模块化,我们扩展Motor1结构体。
然而,查看现有代码,Motor1结构体在foc.h中并没有定义,而是在main.c中定义为全局变量Motor1。因此,我们需要在foc.h中声明Motor1为外部变量,并在foc.h中定义位置环控制所需的新变量(如目标位置、位置使能标志)作为Motor1结构体的成员。
但是,由于原代码中Motor1结构体类型定义在foc.c中(未在头文件中定义),这样其他文件无法访问其成员。因此,我们需要将Motor1结构体类型定义移到foc.h中,并使其成为公共结构体。
由于修改较大,我们采用另一种方法:在foc.c中添加全局变量,并在foc.h中通过extern声明,然后在main.c中通过函数接口设置这些变量。
为了最小化修改,我们选择在foc.c中添加两个全局变量:
float target_angle = 0.0f; // 目标位置(度)
uint8_t position_enabled = 0; // 位置控制使能标志
并在foc.h中声明:
extern float target_angle;
extern uint8_t position_enabled;
然后,在位置环函数Locate_Loop()中,使用这些变量。
步骤1:修改foc.h
添加以下声明:
extern float target_angle;
extern uint8_t position_enabled;
步骤2:在foc.c中定义这些全局变量,并实现Locate_Loop()函数
步骤3:修改main.c中的按键控制,通过按键设置目标位置和使能位置控制。
具体实现如下:</think>### 位置环控制集成方案
我将为您的FOC电机控制系统添加位置环控制功能,保持原有结构不变,仅添加必要的位置控制逻辑。以下是完整的修改方案:
#### 1. 在`foc.h`中添加位置控制声明
```c
/* 添加位置控制声明 */
#ifdef __cplusplus
extern "C" {
#endif
// 位置环控制函数声明
void Locate_Loop(void);
// 位置控制变量声明
extern float target_angle; // 目标位置(度)
extern uint8_t position_enabled; // 位置控制使能标志
#ifdef __cplusplus
}
#endif
```
#### 2. 在`foc.c`中添加位置控制实现
```c
/* 添加位置控制全局变量 */
float target_angle = 0.0f; // 目标位置(度)
uint8_t position_enabled = 0; // 位置控制使能标志
/* 位置环实现 */
void Locate_Loop(void)
{
// 位置控制未使能时直接返回
if (!position_enabled) return;
// 计算位置误差(处理360°边界)
float error = target_angle - Motor1.Theta_m;
// 处理360°圆周上的最短路径
if (error > 180.0f) {
error -= 360.0f;
} else if (error < -180.0f) {
error += 360.0f;
}
// 比例控制输出作为速度指令
const float Kp_position = 0.5f; // 位置环比例增益(可调整)
Motor1.Wm_Set = Kp_position * error;
// 速度限幅(根据电机性能调整)
const float max_speed = 1000.0f; // 最大速度限制
Motor1.Wm_Set = Value_SetMaxMin(Motor1.Wm_Set, -max_speed, max_speed);
// 位置到达检测(误差小于1度视为到达)
if (fabsf(error) < 1.0f) {
Motor1.Wm_Set = 0; // 停止电机
// 可选:到达后自动禁用位置控制
// position_enabled = 0;
}
}
```
#### 3. 修改`main.c`添加位置控制按键功能
```c
/* USER CODE BEGIN PV */
// 位置控制临时变量
static float position_increment = 30.0f; // 位置调整步长
/* USER CODE END PV */
int main(void)
{
// ... 初始化代码保持不变 ...
while (1)
{
// 原有按键功能保持不变...
/* 新增位置控制按键功能 */
if(Key_Scan(KEY1_GPIO_Port, KEY1_Pin) == KEY_ON)
{
// 启动位置控制
position_enabled = 1;
LED_D11_ON;
printf("Position control ON\n");
HAL_Delay(200);
}
else if(Key_Scan(KEY2_GPIO_Port, KEY2_Pin) == KEY_ON)
{
// 停止位置控制
position_enabled = 0;
Motor1.Wm_Set = 0; // 停止电机
LED_D11_OFF;
printf("Position control OFF\n");
HAL_Delay(200);
}
else if(Key_Scan(KEY3_GPIO_Port, KEY3_Pin) == KEY_ON)
{
// 位置增加
target_angle += position_increment;
if(target_angle >= 360.0f) target_angle -= 360.0f;
printf("Target: %.1f°\n", target_angle);
HAL_Delay(200);
}
else if(Key_Scan(KEY4_GPIO_Port, KEY4_Pin) == KEY_ON)
{
// 位置减少
target_angle -= position_increment;
if(target_angle < 0.0f) target_angle += 360.0f;
printf("Target: %.1f°\n", target_angle);
HAL_Delay(200);
}
else if(Key_Scan(KEY5_GPIO_Port, KEY5_Pin) == KEY_ON)
{
// 位置归零
target_angle = 0.0f;
printf("Target set to 0°\n");
HAL_Delay(200);
}
// 位置监控(每200ms输出一次)
static uint32_t last_print = 0;
if(HAL_GetTick() - last_print > 200) {
printf("Target: %.1f° | Actual: %.1f° | Error: %.1f°\n",
target_angle, Motor1.Theta_m,
target_angle - Motor1.Theta_m);
last_print = HAL_GetTick();
}
HAL_Delay(10);
}
}
```
### 位置控制原理说明
#### 1. 控制结构
```
位置环 → 速度环 → 电流环 → PWM输出
↑ ↑ ↑
目标位置 位置环输出 速度环输出
```
#### 2. 关键参数说明
1. **位置环比例增益 (Kp_position)**
```c
const float Kp_position = 0.5f; // 位置环比例增益
```
- 控制位置响应速度
- 值越大响应越快,但可能导致超调
- 推荐范围:0.1~2.0(根据电机性能调整)
2. **位置调整步长**
```c
static float position_increment = 30.0f; // 位置调整步长
```
- 每次按键调整的角度值
- 可根据需要调整为5°、15°、45°等
3. **最大速度限制**
```c
const float max_speed = 1000.0f; // 最大速度限制
```
- 防止位置环输出过大速度指令
- 应根据电机最大转速设置
### 位置环工作流程
1. **位置环调用**
- 在`Foc_Control()`函数中周期性调用
- 调用频率由`Motor1_Time.Locate_Loop_TimeGain`控制
2. **位置误差计算**
```c
float error = target_angle - Motor1.Theta_m;
```
- 计算目标位置与实际位置的差值
- 处理360°边界问题
3. **比例控制**
```c
Motor1.Wm_Set = Kp_position * error;
```
- 输出作为速度环的设定值
- 简单高效的比例控制
4. **速度限幅**
```c
Motor1.Wm_Set = Value_SetMaxMin(Motor1.Wm_Set, -max_speed, max_speed);
```
- 防止速度指令超出电机能力范围
5. **位置到达检测**
```c
if (fabsf(error) < 1.0f) {
Motor1.Wm_Set = 0;
}
```
- 当误差小于1°时停止电机
- 可添加到达提示或自动禁用位置控制
### 优化建议
#### 1. 添加S曲线加减速
```c
// 在位置环中添加S曲线速度规划
void Position_Planner(void)
{
static float current_position = 0.0f;
const float max_accel = 2000.0f; // 最大加速度 (°/s²)
// 计算S曲线速度规划
float position_error = target_angle - current_position;
float direction = (position_error > 0) ? 1.0f : -1.0f;
// 计算加速距离
float accelerate_distance = 0.5f * max_speed * max_speed / max_accel;
// 速度规划
if (fabsf(position_error) > 2 * accelerate_distance) {
// 梯形速度规划
if (fabsf(Motor1.Wm_Set) < max_speed) {
Motor1.Wm_Set += direction * max_accel * Motor1_Time.Locate_Loop_Ts;
}
} else {
// 减速阶段
float decelerate_speed = sqrtf(2 * max_accel * fabsf(position_error));
Motor1.Wm_Set = direction * fminf(fabsf(Motor1.Wm_Set), decelerate_speed);
}
// 更新当前位置
current_position += Motor1.Wm_Set * Motor1_Time.Locate_Loop_Ts;
}
```
#### 2. 抗积分饱和处理
```c
// 在位置环中添加积分项(可选)
static float position_integral = 0.0f;
const float Ki_position = 0.01f; // 积分增益
// 积分项计算
if (fabsf(error) > 5.0f) { // 仅在大误差时积分
position_integral += Ki_position * error * Motor1_Time.Locate_Loop_Ts;
position_integral = Value_SetMaxMin(position_integral, -max_speed/2, max_speed/2);
}
// 输出速度指令
Motor1.Wm_Set = Kp_position * error + position_integral;
```
#### 3. 位置环参数初始化
```c
// 在Foc_Init()中添加位置环参数初始化
void Foc_Init()
{
// ...原有代码...
// 位置环参数初始化
const float position_BandWidth = 10.0f; // 位置环带宽(Hz)
Motor1_Time.Locate_Loop_Kp = position_BandWidth * 2 * PI * MOTOR1_J / (1.5f * MOTOR1_Pn * MOTOR1_flux);
Motor1_Time.Locate_Loop_Ki = position_BandWidth * Motor1_Time.Locate_Loop_Kp;
}
```
### 完整的位置控制流程图
```
按键操作
│
▼
设置目标位置 ────┐
│ │
▼ ▼
位置环控制 ──▶ 速度环控制 ──▶ 电流环控制 ──▶ PWM输出
▲ ▲ ▲
│ │ │
位置反馈 ──────┘ │
│ │
编码器读数 ───────────────┘
```
此实现方案保持了原有的FOC控制结构,仅添加了必要的位置控制逻辑,通过按键可以方便地控制目标位置和启停位置控制。位置环采用简单的比例控制,输出作为速度环的设定值,实现电机的位置控制功能。
阅读全文
相关推荐


















