1、霍尔速度闭环，IF启动

motor/adc.c

@@ -156,16 +156,15 @@ static void uvw_current_and_vbus_get(float* vbus, double* iu, double* iv, double
  */
 void motor_run(void)
 {
-    uvw_current_and_vbus_get(&Vbus, &Ia, &Ib, &Ic);
-    //Hall_TypeDef* hall = Hall_Get();
-		//MotorParam_t* motor = get_motor();
-
-// Hall_Update_Interpolated_Angle();
+	// 采集三相电流 母线电压
+  uvw_current_and_vbus_get(&Vbus, &Ia, &Ib, &Ic);
+  
+	// IF 软起动
   if(speed_close_loop_flag==0)         //速度环闭环切换控制，电机刚启动时速度环不闭环
   {                                    //并且电流参考值缓慢增加（防冲击），速度达到一定值
     if((Iq_ref<MOTOR_STARTUP_CURRENT)) //速度切入闭环
     {                                  //电流环在电机运行过程中全程闭环
-      Iq_ref += 0.00003f;              //角度在电机刚启动时就闭环运行，无需强拖，得益于卡尔曼滤波做
+      Iq_ref += 0.001f;              //角度在电机刚启动时就闭环运行，无需强拖，得益于卡尔曼滤波做
     }                                  //状态观测器低速性能比教好
     else
     {
@@ -176,7 +175,7 @@ void motor_run(void)
   {
     if(speed_close_loop_flag==1)
     {
-      if(Iq_ref>(MOTOR_STARTUP_CURRENT/2.0f))
+      if(Iq_ref>(MOTOR_STARTUP_CURRENT)/2.0f)
       {
         Iq_ref -= 0.001f;
       }
@@ -187,62 +186,47 @@ void motor_run(void)
     }
   }
   
-//     if(FOC_Output.EKF[2]>SPEED_LOOP_CLOSE_RAD_S)      //无感方式运行，状态观测器得到角度和速度信息
-//  {
-//    FOC_Input.Id_ref = 0.0f;
-//    Speed_Fdk = FOC_Output.EKF[2];
-//    FOC_Input.Iq_ref = Speed_Pid_Out;
-//  }
-//  else
-//  {
-//    FOC_Input.Id_ref = 0.0f;
-//    FOC_Input.Iq_ref = Iq_ref;
-//    Speed_Pid.I_Sum = Iq_ref;
-//  }
-//  FOC_Input.theta = FOC_Output.EKF[3];
-//  FOC_Input.speed_fdk = FOC_Output.EKF[2];
-//	EKF_Hz = FOC_Output.EKF[2]/(2.0f*PI);
-//  FOC_Input.Id_ref = 0.0f;               
-//  FOC_Input.Tpwm = PWM_TIM_PULSE_TPWM;         //FOC运行函数需要用到的输入信息
-//  FOC_Input.Udc = Vbus;
-//  FOC_Input.Rs = Rs;
-//  FOC_Input.Ls = Ls;
-//  FOC_Input.flux = flux;
-//  
-//  FOC_Input.ia = Ia;
-//  FOC_Input.ib = Ib;
-//  FOC_Input.ic = Ic;           
- // foc_algorithm_step();       //整个FOC运行函数（包括无感状态观测器，电流环，SVPWM，坐标变换，电机参数识别）
     // ========== FOC参数设置 ==========
     FOC_parm_input();
-		get_foc_input()->Iq_ref = 1.0f;
-		Hall_Update_Interpolated_Angle();
-		static uint16_t cnt = 0;
-		if(cnt <= 10000)
-		{
-			cnt++;
-			theta += 0.01f;
-			if(theta >= 2* PI)
-			{
-				theta -= 2 * PI;
-			}
-			if(theta < 0.0f)
-			{
-				theta += 2 * PI;
-			}
-			get_foc_input()->theta = theta;
-			
-		}else
-		{
-			get_foc_input()->theta = Hall_Get()->angle;
-		}
-			
-  
+
+	
+
+#ifdef HALL_FOC_SELECT	
+	
+		 Hall_Update_Interpolated_Angle(); // 霍尔角度插值
+	
+    if(Hall_Get()->Speed > SPEED_LOOP_CLOSE_RAD_S )     // 速度达到闭环阈值
+    {
+        FOC_Input.Id_ref = 0.0f;
+        Speed_Fdk = Hall_Get()->Speed;
+        FOC_Input.Iq_ref = Speed_Pid_Out;
+				// 切闭环霍尔角度
+				get_foc_input()->theta = Hall_Get()->angle;
+    }
+    else
+    {
+        FOC_Input.Id_ref = 0.0f;
+        FOC_Input.Iq_ref = Iq_ref;
+        Speed_Pid.I_Sum = Iq_ref;
+				// 模拟自增电角度
+				theta += 0.005f; // 自增角度大了影响切换？？ 给到0.01的时候经常滑丝
+				if(theta >= 2* PI)
+				{
+					theta -= 2 * PI;
+				}
+				if(theta < 0.0f)
+				{
+					theta += 2 * PI;
+				}
+				get_foc_input()->theta = theta;
+    }
+    FOC_Input.speed_fdk = Hall_Get()->Speed; // 可用于参数识别
+#endif
     // ========== 执行FOC算法 ==========
-     motor_foc_openloop_run();
-    //motor_ekf_closeloop_run();
+     motor_foc_alg_run();
+  
     // ========== PWM输出更新 ==========
-    if(motor_start_stop == 1 )//&& motor->ctrl_state != CTRL_STATE_IDLE)
+    if(motor_start_stop == 1 )
     {
         PWM_TIM->CCR1 = (u16)(FOC_Output.Tcmp1);
         PWM_TIM->CCR2 = (u16)(FOC_Output.Tcmp2);
@@ -256,11 +240,7 @@ void motor_run(void)
     }
     
     // ========== 停止命令处理 ==========
-//    if(motor_start_stop == 0 && motor->ctrl_state != CTRL_STATE_IDLE && 
-//       motor->ctrl_state != CTRL_STATE_STOPPING)
-//    {
-//        motor->ctrl_state = CTRL_STATE_STOPPING;
-//    }
+
 }
 
 
@@ -310,20 +290,20 @@ void motor_run(void)
     // 根据传感器类型选择FOC控制模式  
 #ifdef  HALL_FOC_SELECT            // 使用霍尔传感器的FOC控制  
     
-//    if((hall_speed*2.0f*PI)>SPEED_LOOP_CLOSE_RAD_S)     // 速度达到闭环阈值
-//    {
-//        FOC_Input.Id_ref = 0.0f;
-//        Speed_Fdk = hall_speed*2.0f*PI;
-//        FOC_Input.Iq_ref = Speed_Pid_Out;
-//    }
-//    else
-//    {
-//        FOC_Input.Id_ref = 0.0f;
-//        FOC_Input.Iq_ref = Iq_ref;
-//        Speed_Pid.I_Sum = Iq_ref;
-//    }
-//    FOC_Input.theta = hall_angle;
-//    FOC_Input.speed_fdk = hall_speed*2.0f*PI;
+    if((hall_speed*2.0f*PI)>SPEED_LOOP_CLOSE_RAD_S)     // 速度达到闭环阈值
+    {
+        FOC_Input.Id_ref = 0.0f;
+        Speed_Fdk = hall_speed*2.0f*PI;
+        FOC_Input.Iq_ref = Speed_Pid_Out;
+    }
+    else
+    {
+        FOC_Input.Id_ref = 0.0f;
+        FOC_Input.Iq_ref = Iq_ref;
+        Speed_Pid.I_Sum = Iq_ref;
+    }
+    FOC_Input.theta = hall_angle;
+    FOC_Input.speed_fdk = hall_speed*2.0f*PI;
     
 #endif
     

motor/foc_algorithm.c

@@ -18,19 +18,19 @@
 // 注：Vq和Vd是控制器输出，用于控制电机的q轴和d轴电压
 // Iq和Id是反馈电流，分别控制电机的转矩和磁场
 // 控制器参数通过自动调优获得
-real32_T D_PI_I = 1282.8F;          // D轴积分系数
+real32_T D_PI_I = CURRENT_KI;          // D轴积分系数
 real32_T D_PI_KB = 15.0F;         // D轴反馈系数 （抗饱和积分项）
 real32_T D_PI_ISUM_MAX = 12.0f;   // D轴最大积分限幅
 real32_T D_PI_ISUM_MIN = -12.0f;  // D轴最小积分限幅
 real32_T D_PI_LOW_LIMIT = -24.0F; // D轴输出下限
-real32_T D_PI_P = 2.199F;          // D轴比例系数
+real32_T D_PI_P = CURRENT_KP;          // D轴比例系数
 real32_T D_PI_UP_LIMIT = 24.0F;   // D轴输出上限
-real32_T Q_PI_I = 1282.8F;          // Q轴积分系数
+real32_T Q_PI_I = CURRENT_KI;          // Q轴积分系数
 real32_T Q_PI_ISUM_MAX = 12.0f;   // Q轴最大积分限幅
 real32_T Q_PI_ISUM_MIN = -12.0f;  // Q轴最小积分限幅
 real32_T Q_PI_KB = 15.0F;         // Q轴反馈系数
 real32_T Q_PI_LOW_LIMIT = -24.0F; // Q轴输出下限
-real32_T Q_PI_P = 2.199F;         // Q轴比例系数
+real32_T Q_PI_P = CURRENT_KP;         // Q轴比例系数
 real32_T Q_PI_UP_LIMIT = 24.0F;   // Q轴输出上限
 
 

motor/foc_algorithm.h

@@ -62,7 +62,7 @@ typedef struct {
 typedef struct {
   real32_T Id_ref;               // D轴电流参考值
   real32_T Iq_ref;               // Q轴电流参考值
-  real32_T speed_fdk;            // 速度反馈值
+  real32_T speed_fdk;            // 速度反馈值 电弧度
   real32_T theta;                // 电机角度
   real32_T ia;                   // A相电流
   real32_T ib;                   // B相电流
@@ -291,7 +291,7 @@ extern void motor_ekf_closeloop_run(void);
 
 extern void motor_hall_close_run(void);
 
-extern void motor_foc_openloop_run(void);
+extern void motor_foc_alg_run(void);
 
 extern FOC_INPUT_DEF* get_foc_input(void);
 

motor/foc_define_parameter.h

@@ -17,15 +17,23 @@
 #define VBUS 24.0f
 
 // 选择FOC类型：霍尔传感器FOC或无传感器FOC
-//#define HALL_FOC_SELECT          // 使用霍尔传感器的FOC模式
-#define SENSORLESS_FOC_SELECT    // 使用无传感器的FOC模式
+#define HALL_FOC_SELECT          // 使用霍尔传感器的FOC模式
+// #define SENSORLESS_FOC_SELECT    // 使用无传感器的FOC模式
 
 // 电机参数（基于TG5P40电机）
 #define RS_PARAMETER     0.445f           // 定子电阻 (Ω)
 #define LS_PARAMETER     0.00031f         // 定子电感 (H)
 #define FLUX_PARAMETER   0.007797f        // 磁链 (Wb)
-
-
+// 表贴式d轴和q轴的电流环参数一致
+#define CURRENT_KP (float)(2*PI*RS_PARAMETER)
+#define CURRENT_KI (float)(2*PI*RS_PARAMETER*RS_PARAMETER/LS_PARAMETER)
+	
+#define POLES   (4)
+#define MAX_RPM (3000)
+#define MAX_ELE_RAD (float)(MAX_RPM/60*POLES*2*PI)
+	
+#define MAX_ELE_RPS  (float) (3000*4.0f)
+#define MIN_ELE_RPS  (float) (15.0f) 
 //    % TG5P40电机参数
 //    % MATLAB参数设置
 //    MOTOR_PNF_param.Value = 4;          % 电机极对数

motor/hall_sensor.c

@@ -123,3 +123,33 @@ void Hall_Update_Interpolated_Angle(void)
     if(Hall.angle >= 2*PI) Hall.angle -= 2*PI;
     if(Hall.angle < 0) Hall.angle += 2*PI;
 }
+
+
+void Hall_Init_For_Startup(void)
+{
+    // 清零速度数组
+    for(int i = 0; i < 6; i++)
+    {
+        Hall.speed[i] = 0.0f;
+        Hall.theta[i] = 0.0f;
+        Hall.count[i] = 0;
+    }
+    
+    // 清零速度历史
+    for(int i = 0; i < SPEED_FILTER_DEPTH; i++)
+        speed_history[i] = 0.0f;
+    speed_idx = 0;
+    
+    // 重置角度增量
+    Hall.angle_add = 0.0f;
+		Hall.Speed = 0.0f;
+		// 重置计数值
+    Hall.ccr = 0;
+		
+    // 强制获取当前霍尔状态
+    uint8_t W = GPIO_ReadInputDataBit(HALL_CH3_GPIO_PORT, HALL_CH3_PIN);
+    uint8_t V = GPIO_ReadInputDataBit(HALL_CH2_GPIO_PORT, HALL_CH2_PIN);
+    uint8_t U = GPIO_ReadInputDataBit(HALL_CH1_GPIO_PORT, HALL_CH1_PIN);
+    Hall.state = U | (V << 1) | (W << 2);
+    Hall.state_last = Hall.state;
+}

motor/hall_sensor.h

@@ -69,4 +69,6 @@ extern Hall_TypeDef* Hall_Get(void);
 extern void Hall_Update_Interpolated_Angle(void);   // 更新插值角度
 
 extern float Get_Filtered_Speed(void); // 速度滤波
+
+extern void Hall_Init_For_Startup(void); // 霍尔传感器清零
 #endif

motor/low_task.c

@@ -22,28 +22,23 @@ void motor_start(void)
   foc_algorithm_initialize();
   
   // 设置速度参考值为20.0转/秒
-  Speed_Ref = 20.0F;
+  Speed_Ref = 20.0F; // 20*6.28 也就是125.6 erad/s  5转每秒
   
   // 清除速度闭环标志
   speed_close_loop_flag = 0;
   
   // 设置q轴电流参考值
   Iq_ref = 0.0f;
-
-//  // 初始化电机角度增量
-//  hall_angle_add = 0.0005f;
-//  
-//  // 初始化电机速度
-//  hall_speed = 0.0f;
 	
   SHUTDOWN_EN();
+	
   // 使能PWM输出
   TIM_CtrlPWMOutputs(PWM_TIM, ENABLE);
   
   // 通知LED应用电机启动
   motor_state_callback(1);
-  
-  //motor_run_display_flag = 1;  // 显示标志（已注释）
+  // 清除霍尔异常计数值
+  Hall_Init_For_Startup();
 }
 
 /**
@@ -131,16 +126,19 @@ void low_control_task(void)
   // 处理KEY2按键事件
   if(key2_flag == 1)
   {
-    Speed_Ref += 5.0f;  // 速度参考值增加5.0转/秒
-		Iq_ref += 0.2f;
+   	if(Speed_Ref < MAX_ELE_RPS)
+			Speed_Ref += 5.0f;  // 速度参考值减少5.0转/秒
+		// Iq_ref += 0.2f;
     key2_flag = 0;  // 清除按键标志
   }
   
   // 处理KEY3按键事件
   if(key3_flag == 1)
   {
-    Speed_Ref -= 5.0f;  // 速度参考值减少5.0转/秒
-		Iq_ref -= 0.2f;
+		if(Speed_Ref > MIN_ELE_RPS)
+			Speed_Ref -= 5.0f;  // 速度参考值减少5.0转/秒
+		
+		// Iq_ref -= 0.2f;
     key3_flag = 0;      // 清除按键标志
   }
 }

motor/motor_run.c

@@ -280,7 +280,7 @@ void motor_hall_close_run(void)
 
 }
 
-void motor_foc_openloop_run(void)
+void motor_foc_alg_run(void)
 {
 	
 		// 1. ¶ÁÈ¡ÈýÏàµçÁ÷

motor/speed_pid.c

@@ -18,7 +18,7 @@
 real32_T SPEED_PI_I = 5.0F;           // 积分系数
 real32_T SPEED_PI_KB = 0.015F;        // 反馈系数
 real32_T SPEED_PI_LOW_LIMIT = -4.0F;  // 输出下限
-real32_T SPEED_PI_P = 0.003F;         // 比例系数
+real32_T SPEED_PI_P = 0.004F;         // 比例系数
 real32_T SPEED_PI_UP_LIMIT = 4.0F;    // 输出上限
 
 /**

user/main.c

@@ -73,7 +73,7 @@ int main(void)
   while (1)
   {
       
-		#define PRINT_LEN 11
+		#define PRINT_LEN 13
 			float data_print[PRINT_LEN] = {0};
 			const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f};  // 帧尾
         // 准备打印数据并直接发送到 DMA 缓冲区
@@ -107,18 +107,45 @@ int main(void)
 //			data_print[15] = Hall_Get()->angle_add;
 //			data_print[16] = theta;
 
-			data_print[0] = Hall_Get()->speed[0];
-			data_print[1] =  Hall_Get()->speed[1];
-			data_print[2] = Hall_Get()->speed[2];
-			data_print[3] = Hall_Get()->speed[3];
-			data_print[4] = Hall_Get()->speed[4];
-			data_print[5] = Hall_Get()->speed[5];
-			data_print[6] = Hall_Get()->state;               
-			data_print[7] = Hall_Get()->Speed;     
-			data_print[8] = Hall_Get()->angle;
-			data_print[9] = Hall_Get()->angle_add;
-			data_print[10] = theta;
+//			data_print[0] = Hall_Get()->speed[0];
+//			data_print[1] =  Hall_Get()->speed[1];
+//			data_print[2] = Hall_Get()->speed[2];
+//			data_print[3] = Hall_Get()->speed[3];
+//			data_print[4] = Hall_Get()->speed[4];
+//			data_print[5] = Hall_Get()->speed[5];
+//			data_print[6] = Hall_Get()->state;               
+//			data_print[7] = Hall_Get()->Speed;     
+//			data_print[8] = Hall_Get()->angle;
+//			data_print[9] = Hall_Get()->angle_add;
+//			data_print[10] = theta;
 			
+//			data_print[0] = get_curr_dq()->Iq;
+//			data_print[1] =  get_foc_input()->Iq_ref;
+//			data_print[2] = get_curr_dq()->Id;
+//			data_print[3] = get_foc_input()->Id_ref;
+//			data_print[4] = FOC_Input.ia;
+//			data_print[5] = FOC_Input.ib;
+//			data_print[6] = FOC_Input.ic;
+//			data_print[7] = Hall_Get()->state;               
+//			data_print[8] = Hall_Get()->Speed;     
+//			data_print[9] = Hall_Get()->angle;
+//			data_print[10] = Hall_Get()->angle_add;
+//			data_print[11] = theta;
+//			data_print[12] = MAX_ELE_RAD;
+			
+			data_print[0] = get_curr_dq()->Iq;
+			data_print[1] = get_foc_input()->Iq_ref;
+			data_print[2] = get_curr_dq()->Id;
+			data_print[3] = get_foc_input()->Id_ref;
+			data_print[4] = FOC_Input.ia;
+			data_print[5] = FOC_Input.ib;
+			data_print[6] = FOC_Input.ic;
+			data_print[7] = Speed_Fdk / 2.0f / PI;               
+			data_print[8] = Speed_Ref;     
+			data_print[9] = Hall_Get()->angle;
+			data_print[10] = Hall_Get()->angle_add;
+			data_print[11] = theta;
+			data_print[12] = MAX_ELE_RAD;
 #else
 //      data_print[0] = FOC_Input.ia;
 //      data_print[1] = FOC_Input.ib;