扭矩模式限速通过PI调节器限制DQ的最大输出电压

Signed-off-by: huhui <huhui@sharkgulf.com>

Applications/app/app.c

@@ -116,7 +116,7 @@ static u32 _app_report_task(void *p) {
 	}
 	return 200;
 }
-static int plot_type = 7;
+static int plot_type = 1;
 static void plot_smo_angle(void) {
 	float smo_angle = foc_observer_smo_angle();
 	float delta = smo_angle - PMSM_FOC_Get()->in.s_hallAngle;
@@ -130,7 +130,7 @@ static void plot_smo_angle(void) {
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2(PMSM_FOC_GetSpeed() + 50, PMSM_FOC_GetSpeed());
+		can_plot2((s16)(PMSM_FOC_Get()->params.f_DCLim*10.0f), (s16)PMSM_FOC_GetSpeed());
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_RefTorque(), PMSM_FOC_Get_Real_Torque());
 	}else if (plot_type == 3) {
@@ -140,12 +140,12 @@ static u32 _app_plot_task(void * args) {
 		can_plot2(eCtrl_get_RefTorque(), PMSM_FOC_Get()->in.s_targetTorque);
 	}else if (plot_type == 5) {
 #ifdef CONFIG_DQ_STEP_RESPONSE
-		can_plot2((s16)(target_d), (s16)(PMSM_FOC_Get()->out.s_RealIdq.d));
+		can_plot2((s16)(target_d*10.0f), (s16)(PMSM_FOC_Get()->out.s_RealIdq.d * 10.0f));
 #endif
 	}else if (plot_type == 6) {
 		can_plot2((s16)(PMSM_FOC_Get()->in.s_iABC[0]), (s16)(PMSM_FOC_Get()->in.s_iABC[1]));
 	}else if (plot_type == 7) {
-		can_plot2((s16)PMSM_FOC_Get()->out.s_OutVdq.d, (s16)PMSM_FOC_Get()->out.s_FilterIdq.d);
+		can_plot2((s16)PMSM_FOC_Get()->out.s_FilterIdq.d, (s16)PMSM_FOC_Get()->out.s_FilterIdq.q);
 	}
 	
 	return 20;

Applications/bsp/board_mc100_v1.h

@@ -23,7 +23,6 @@
 #define CONFIG_UNDER_VOL_RPM     1000
 #define CONFIG_UNDER_VOL_PHASE_CURR 100.0F
 #define CONFIG_UNDER_VOL_DC_CURR 15.0F
-#define CONFIG_MAX_FW_D_CURR     150.0F //d轴最大的退磁电流
 
 #define CONFIG_CURR_LP_WC (600.0F)
 

Applications/foc/commands.h

@@ -40,7 +40,7 @@ typedef enum {
 	Foc_End_Write_TRQ_Table,
 	Foc_SN_Write, //66
 	Foc_SN_Read,
-	Foc_Set_DQ_Current,
+	Foc_Set_DQ_Current, //0x44
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PI_Controller.h

@@ -29,8 +29,6 @@ typedef struct {
 	float  kp;
 	float  ki;
 	float  kb;
-	//float  kp_s;
-	//float  ki_s;
 	float  max;
 	float  min;
 	float  Ui;
@@ -49,7 +47,7 @@ static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->is_sat = false;
 }
 
-static __INLINE float PI_Controller_run(PI_Controller *pi, float err) {
+static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
 	float integral = ki_err * pi->DT;

Applications/foc/core/PMSM_FOC_Core.c

@@ -226,7 +226,7 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.in.s_manualAngle = INVALID_ANGLE;
 	gFoc_Ctrl.in.b_fwEnable = nv_get_foc_params()->n_FwEnable;
 	gFoc_Ctrl.in.s_vDC = nv_get_foc_params()->s_maxDCVol;//(CONFIG_RATED_DC_VOL);
-
+	gFoc_Ctrl.params.f_DCLim = get_vbus_float();
 	eRamp_init_target(&gFoc_Ctrl.in.cruiseRpmRamp, 0, CONFIG_ACC_TIME, CONFIG_DEC_TIME);
 	
 	gFoc_Ctrl.out.n_RunMode = CTRL_MODE_OPEN;
@@ -308,11 +308,17 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 
 static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	/* update id pi ctrl */
-	gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-	gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
-
+	if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
+		gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+	}else {
+		gFoc_Ctrl.params.maxvDQ.d = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.minvDQ.d = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+		gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.in.s_vDC * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
+	}
 	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id->max) {
 		gFoc_Ctrl.pi_id->max = gFoc_Ctrl.params.maxvDQ.d;
 	}
@@ -507,28 +513,23 @@ static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 #if 1
 	gFoc_Ctrl.pi_power->max = maxTrq;
 	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.DCCurrLimRamp) - (gFoc_Ctrl.out.s_FilteriDC);
-	return PI_Controller_run(gFoc_Ctrl.pi_power, errRef);
+	return PI_Controller_Run(gFoc_Ctrl.pi_power, errRef);
 #else
 	return maxTrq;
 #endif
 }
 
-static __INLINE float PMSM_FOC_Limit_Speed(float refTorque) {
+static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 #if 1
-	if (refTorque >= 0) {
-		gFoc_Ctrl.pi_torque->max = refTorque;
-		gFoc_Ctrl.pi_torque->min = -CONFIG_MAX_NEG_TORQUE;
-	}else {
-		gFoc_Ctrl.pi_torque->min = refTorque;
-		gFoc_Ctrl.pi_torque->max = 0;
-	}
-
-	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
-	float trq_res = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
+	gFoc_Ctrl.pi_torque->max = gFoc_Ctrl.in.s_vDC;
+	gFoc_Ctrl.pi_torque->min = 0;
 
-	return trq_res;
+	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
+	gFoc_Ctrl.params.f_DCLim = PI_Controller_Run(gFoc_Ctrl.pi_torque, err);
+	return maxTrq;
 #else
-	return refTorque;
+	gFoc_Ctrl.params.f_DCLim = gFoc_Ctrl.in.s_vDC;
+	return maxTrq;
 #endif
 }
 
@@ -566,7 +567,7 @@ void PMSM_FOC_idqCalc(void) {
 		gFoc_Ctrl.pi_lock->min = -CONFIG_DEFAULT_LOCK_TORQUE_LIM;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
-		gFoc_Ctrl.in.s_targetTorque = PI_Controller_run(gFoc_Ctrl.pi_lock ,errRef);
+		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(gFoc_Ctrl.pi_lock ,errRef);
 		PMSM_FOC_idq_Assign();
 		return;
 	}
@@ -603,7 +604,7 @@ void PMSM_FOC_idqCalc(void) {
 		}
 		gFoc_Ctrl.in.s_targetRPM = refSpeed;
 		float errRef = refSpeed - gFoc_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_speed, errRef);
+		float maxTrq = PI_Controller_Run(gFoc_Ctrl.pi_speed, errRef);
 		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
 	}
 
@@ -633,10 +634,10 @@ void PMSM_FOC_Slow_Task(void) {
 }
 
 float PMSM_FOC_Get_Real_Torque(void) {
-	if (gFoc_Ctrl.out.s_RealCurrent == 0) {
-		gFoc_Ctrl.out.s_RealCurrent = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
+	if (gFoc_Ctrl.out.s_RealCurrentFiltered == 0) {
+		gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 	}
-	return gFoc_Ctrl.out.s_RealCurrent;
+	return gFoc_Ctrl.out.s_RealCurrentFiltered;
 }
 
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
@@ -1028,7 +1029,7 @@ void PMSM_FOC_Calc_Current(void) {
 	raw_idc = get_vbus_current();
 	LowPass_Filter(gFoc_Ctrl.out.s_FilteriDC, raw_idc, 0.1f);
 
-	gFoc_Ctrl.out.s_RealCurrent = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
+	gFoc_Ctrl.out.s_RealCurrentFiltered = sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -75,6 +75,7 @@ typedef struct {
 	float n_modulation;
 	float n_PhaseFilterCeof;
 	//float n_TrqVelLimGain;
+	float f_DCLim;
 	DQ_t  maxvDQ;
 	DQ_t  minvDQ;
 }FOC_Params;
@@ -156,7 +157,7 @@ typedef struct {
 	DQ_t  s_FilterIdq;
 	float s_FilteriDC;
 	float s_CalciDC;
-	float s_RealCurrent;
+	float s_RealCurrentFiltered;
 	float f_vdqRation;
 	s16   test_sample;
 	float sin;

Applications/foc/motor/motor.c

@@ -494,6 +494,7 @@ void mc_force_run_open(s16 vd, s16 vq) {
 	phase_current_offset_calibrate();
 	pwm_start();
 	adc_start_convert();
+	pwm_enable_channel();
 	phase_current_calibrate_wait();
 	PMSM_FOC_Set_Angle(0);
 	PMSM_FOC_SetOpenVdq(vd, 0);

Applications/foc/motor/motor_param.h

@@ -23,15 +23,16 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define TRQ_PI_KI 0.15F
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+#define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
 #define MOTOR_NR 0x11
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
-#define MOTOR_R   0.018f
+#define MOTOR_R   0.022f
 #define MOTOR_Ld (0.000140F*0.5f)
-#define MOTOR_Lq (0.000300f*0.5f)
+#define MOTOR_Lq (0.000340f*0.5f)
 #define MOTOR_POLES  4
 #define MOTOR_ENC_OFFSET 0.0F
-#define CONFIG_MAX_MOTOR_TORQUE 40.0F
+#define CONFIG_MAX_MOTOR_TORQUE 200.0F
 
 #define TRQ_PI_KP 0.14F
 #define TRQ_PI_KI 0.15F
@@ -39,6 +40,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_NR 0x16
 
 #define CONFIG_CURRENT_BANDWITH  2000.0f /* 电流环带宽 */
+#define CONFIG_MAX_FW_D_CURR     150.0F //d轴最大的退磁电流
 
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_NEW2
 #define MOTOR_R   0.013f
@@ -54,6 +56,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_NR 0x12
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+#define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
 #define MOTOR_R   0.008f
@@ -69,6 +72,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_NR 0x13
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+#define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
 #elif CONFIG_MOT_TYPE==MOTOR_BLUESHARK_OLD
 #define MOTOR_R 0.012f
@@ -84,6 +88,7 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_NR 0x14
 
 #define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
+#define CONFIG_MAX_FW_D_CURR     100.0F //d轴最大的退磁电流
 
 #elif CONFIG_MOT_TYPE==MOTOR_3505
 //编码器电机 3505

Applications/os/heap_4.c

@@ -38,7 +38,7 @@
 
 #define portBYTE_ALIGNMENT			8
 #define portBYTE_ALIGNMENT_MASK 	0x0007
-#define configTOTAL_HEAP_SIZE    (16*1024)
+#define configTOTAL_HEAP_SIZE    (15*1024)
 #define configASSERT(x)
 /* Block sizes must not get too small. */
 #define heapMINIMUM_BLOCK_SIZE	( ( size_t ) ( xHeapStructSize << 1 ) )

Applications/prot/can_foc_msg.c

@@ -97,7 +97,7 @@ void can_report_mpta_values(u8 can) {
 		return;
 	}
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_MTPA_DQ_Angle));
-	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->out.s_RealCurrent));
+	encode_s16(data + 2, S16Q5(PMSM_FOC_Get()->out.s_RealCurrentFiltered));
 	encode_s16(data + 4, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.d));
 	encode_s16(data + 6, S16Q5(PMSM_FOC_Get()->out.s_FilterIdq.q));
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);