1. 处理转把

2. 减小打齿

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -379,6 +379,11 @@ void PMSM_FOC_Schedule(void) {
 	pwm_update_duty(gFoc_Ctrl.out.n_Duty[0], gFoc_Ctrl.out.n_Duty[1], gFoc_Ctrl.out.n_Duty[2]);
 	pwm_update_sample(gFoc_Ctrl.out.n_Sample1, gFoc_Ctrl.out.n_Sample2, gFoc_Ctrl.out.n_CPhases);
 
+#ifdef NO_SAMPLE_IDC
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.01f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.01f);
+#endif
+
 	if (gFoc_Ctrl.plot_type != Plot_None) {
 		if (gFoc_Ctrl.ctrl_count % 5 == 0) {
 			if (gFoc_Ctrl.plot_type == Plot_Phase_curr) {
@@ -491,10 +496,10 @@ void PMSM_FOC_idqCalc(void) {
 		float refTorque = min(eCtrl_get_RefTorque(), eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp));
 		if (refTorque >= 0) {
 			gFoc_Ctrl.pi_torque->max = refTorque;
-			gFoc_Ctrl.pi_torque->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+			gFoc_Ctrl.pi_torque->min = 0;//-gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
 		}else {
 			gFoc_Ctrl.pi_torque->min = refTorque;
-			gFoc_Ctrl.pi_torque->max = gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+			gFoc_Ctrl.pi_torque->max = 0;//gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
 		}
 		float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
 		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
@@ -557,7 +562,7 @@ void PMSM_FOC_Slow_Task(void) {
 }
 
 float PMSM_FOC_Get_Real_Torque(void) {
-	return sqrtf(SQ(gFoc_Ctrl.out.s_RealIdq.d) + SQ(gFoc_Ctrl.out.s_RealIdq.q)) * 1.1f;
+	return sqrtf(SQ(gFoc_Ctrl.out.s_FilterIdq.d) + SQ(gFoc_Ctrl.out.s_FilterIdq.q));
 }
 
 PMSM_FOC_Ctrl *PMSM_FOC_Get(void) {
@@ -825,11 +830,13 @@ void PMSM_FOC_AutoHold(bool lock) {
 		motor_encoder_lock_pos(lock);
 		PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
 		if (!lock) {
+			//解锁后为了防止倒溜，需要把当前
 			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 				PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
 				PI_Controller_Reset(gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
 			}
+			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
 		}
 		gFoc_Ctrl.in.b_AutoHold = lock;
 	}
@@ -907,10 +914,7 @@ void PMSM_FOC_Set_PlotType(Plot_t t) {
 float PMSM_FOC_Calc_iDC(void) {
 	float vd = gFoc_Ctrl.out.s_OutVdq.d;
 	float vq = gFoc_Ctrl.out.s_OutVdq.q;
-#ifdef NO_SAMPLE_IDC
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.01f);
-	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.01f);
-#endif
+
 	float id = gFoc_Ctrl.out.s_FilterIdq.d;
 	float iq = gFoc_Ctrl.out.s_FilterIdq.q;
     /*

Applications/foc/core/e_ctrl.c

@@ -58,7 +58,7 @@ bool eCtrl_enable_eBrake(bool enable) {
 		g_eCtrl.is_ebrake_shadow = false;
 		return g_eCtrl.is_ebrake_shadow;
 	}
-	if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE) && (PMSM_FOC_Get_Real_Torque() > CONFIG_MIN_CURRENT_FOR_EBRK))) {
+	if (enable && ((PMSM_FOC_GetSpeed() > CONFIG_MIN_RPM_FOR_EBRAKE))) {
 		g_eCtrl.is_ebrake_shadow = true;
 	}else if (!enable && !_eCtrl_isHwBrk_shutPower()){
 		g_eCtrl.is_ebrake_shadow = false;
@@ -79,7 +79,7 @@ void _eCtrl_process_eBrake(void) {
 	_eCtrl_clear_ramp();
 	if (g_eCtrl.is_ebrake) {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
-		eRamp_reset_target(&g_eCtrl.current, PMSM_FOC_Get()->in.s_targetTorque);
+		eRamp_reset_target(&g_eCtrl.current, min(PMSM_FOC_Get_Real_Torque() * 0.9f, eCtrl_get_RefTorque()));
 		eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
 	}else {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
@@ -125,6 +125,7 @@ void eCtrl_Running(void) {
 
 void eCtrl_Reset(void) {
 	_eCtrl_clear_ramp();
+	g_eCtrl.is_ebrake_shadow = g_eCtrl.is_ebrake = false;
 }
 
 static bool _eCtrl_isHwBrk_shutPower(void) {

Applications/foc/core/e_ctrl.h

@@ -124,7 +124,7 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
 }
 
 static void eRamp_X2_running(e_Ramp *r) {
-#if 0
+#if 1
 	if(r->target == r->interpolation) {
 		return;
 	}
@@ -158,16 +158,16 @@ static void eRamp_X2_running(e_Ramp *r) {
 }
 
 static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
-#if 0
+#if 1
 	float c_now = eRamp_get_intepolation(ramp);
 
 	float delta = c - c_now;
 	if (delta > 0) {
-		ramp->first_target = 0;//min(delta, 10.0f);
-		ramp->step_val = 0.01f;
+		ramp->first_target = min(delta, 10.0f);
+		ramp->step_val = 0.02f;
 		ramp->A = (delta - ramp->first_target)/SQ(ramp->acct);
 	}else {
-		ramp->first_target = 0;//MAX(delta, -10.0f);
+		ramp->first_target = MAX(delta, -5.0f);
 		ramp->step_val = -0.01f;
 		ramp->A = (delta - ramp->first_target)/SQ(ramp->dect);
 	}

Applications/foc/core/torque.c

@@ -12,6 +12,10 @@ static torque_lut_t *_trq_tbl = NULL;
 static torque_manager_t g_trq_mn;
 void torque_init(void) {
 	_trq_tbl = nv_get_trq_tlb();
+	torque_reset();
+}
+
+void torque_reset(void) {
 	memset(&g_trq_mn, 0, sizeof(g_trq_mn));
 }
 
@@ -46,29 +50,89 @@ void torque_get_idq(float torque, float rpm, DQ_t *dq_out) {
 
 }
 
-float throttle_to_speed(float f_throttle) {
-
+/* 获取油门开度 */
+static float throttle_ration(float f_throttle) {
 	if (f_throttle <= nv_get_foc_params()->n_minThroVol) {
 		return 0;
 	}
 	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
 
-	float ration = delta / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
+	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
+	
+	return ((float)ration)/100.0f;
+}
 
-	return (PMSM_FOC_GetSpeedLimit() * ration);
+float throttle_to_speed(float f_throttle) {
+	return (PMSM_FOC_GetSpeedLimit() * throttle_ration(f_throttle));
 }
 
 float throttle_to_torque(float f_throttle) {
-	if (f_throttle <= (nv_get_foc_params()->n_minThroVol)) {
-		return 0;
+	return PMSM_FOC_GetTorqueLimit() * throttle_ration(f_throttle);
+}
+
+void torque_mode_process(float f_throttle) {
+	float thro_curr = throttle_ration(f_throttle);
+	float thro_prev = g_trq_mn.thro_prev;
+	float trq_ref = throttle_to_torque(f_throttle);
+
+	if (mc_throttle_released()) {
+		if ((eCtrl_get_FinalTorque() > 0.0f)) {
+			eCtrl_enable_eBrake(true);
+			float ref_now = min(PMSM_FOC_Get_Real_Torque() * 0.9f, eCtrl_get_RefTorque());
+			eCtrl_reset_Torque(ref_now);
+			PMSM_FOC_Set_Torque(0);
+			g_trq_mn.accl = false;
+			g_trq_mn.thro_prev = 0.0f;
+		}
+		return;
 	}
-	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
 
-	float ration = delta / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
-	int torque = PMSM_FOC_GetTorqueLimit() * ration * 10.0f;
-	return ((float)torque / 10.0f);
+	if (thro_curr > thro_prev) {
+		if (PMSM_FOC_GetSpeed() == 0.0f) {
+			g_trq_mn.accl_ref = MAX(eCtrl_get_FinalTorque(), trq_ref); //不能小于autohold产生的扭矩
+		}else if (!g_trq_mn.accl){
+			g_trq_mn.accl_ref = eCtrl_get_RefTorque();
+		}
+		if (g_trq_mn.accl_ref > PMSM_FOC_GetTorqueLimit()) {
+			g_trq_mn.accl_ref = PMSM_FOC_GetTorqueLimit();
+		}
+		trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);		
+
+		g_trq_mn.accl = true;
+	}else if (thro_curr < thro_prev){
+		if (g_trq_mn.accl) {
+			g_trq_mn.accl_ref = min(PMSM_FOC_Get_Real_Torque() * 0.9f, eCtrl_get_RefTorque());
+			eCtrl_reset_Torque(g_trq_mn.accl_ref);
+		}
+		trq_ref = thro_curr * g_trq_mn.accl_ref;
+		g_trq_mn.accl = false;
+		
+	}
+	PMSM_FOC_Set_Torque(trq_ref);
+	g_trq_mn.thro_prev = thro_curr;
+}
+
+void speed_mode_process(float f_throttle) {
+	float speed_Ref = throttle_to_speed(f_throttle);
+	PMSM_FOC_Set_Speed(speed_Ref);
 }
 
+void throttle_process(u8 run_mode, float f_throttle) {
+	if ((g_trq_mn.count++ % 20) != 0) {
+		return;
+	}
+	if (run_mode == CTRL_MODE_TRQ) {
+		torque_mode_process(f_throttle);
+	}else if (run_mode == CTRL_MODE_SPD) {
+		speed_mode_process(f_throttle);
+	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
+		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
+			eCtrl_enable_eBrake(false);
+		}
+	}
+}
+
+/*
 void torque_manager(u8 run_mode, float f_throttle) {
 	if ((g_trq_mn.count++ % 20) != 0) {
 		return;
@@ -92,5 +156,5 @@ void torque_manager(u8 run_mode, float f_throttle) {
 			eCtrl_enable_eBrake(false);
 		}
 	}
-}
+} */
 

Applications/foc/core/torque.h

@@ -3,17 +3,22 @@
 #include "os/os_types.h"
 #include "foc/core/PMSM_FOC_Core.h"
 typedef struct {
-	float throttle_prev;
+	float thro_prev;
 	float speed_prev;
 	float torque_prev;
+	float trq_start;
+	float trq_accl;
 	u32   timestamp; //ms
 	float toruqe_curr_max; //当前转把对应的最大扭矩
 	float speed_curr_max;   //当前转把对应的最大速度
+	bool  accl; //加速或者减速
+	float accl_ref;
 	u32   count;
 }torque_manager_t;
 void torque_init(void);
+void torque_reset(void);
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out);
-void torque_manager(u8 run_mode, float f_throttle);
+void throttle_process(u8 run_mode, float f_throttle);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/foc_config.h

@@ -22,7 +22,7 @@
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
 
 #define CONFIG_MIN_RPM_FOR_EBRAKE 800 //进入电流回收的最小转速
-#define CONFIG_MIN_CURRENT_FOR_EBRK 10.0F
+#define CONFIG_MIN_CURRENT_FOR_EBRK 100.0F
 #define CONFIG_MIN_RPM_EXIT_EBRAKE 100 //推出电流回收的最小转速
 
 #define CONFIG_IDQ_CTRL_TS FOC_PWM_FS

Applications/foc/motor/motor.c

@@ -132,6 +132,7 @@ bool mc_start(u8 mode) {
 
 	_mc_internal_init(mode, true);
 
+	torque_reset();
 	eCtrl_init(nv_get_foc_params()->n_acc_time, nv_get_foc_params()->n_dec_time);
 	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
@@ -598,7 +599,7 @@ static bool mc_run_stall_process(u8 run_mode) {
 				if (get_delta_ms(motor.runStall_time) >= CONFIG_STALL_MAX_TIME) {
 					motor.b_runStall = true;
 					motor.runStall_time = 0;
-					torque_manager(run_mode, 0.0f);
+					throttle_process(run_mode, 0.0f);
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {
@@ -698,6 +699,7 @@ void Sched_MC_mTask(void) {
 			if (!PMSM_FOC_Is_Start() && (!mc_throttle_released())) {
 				mask = cpu_enter_critical();
 				PMSM_FOC_Start(motor.mode);
+				torque_reset();
 				pwm_enable_channel();
 				cpu_exit_critical(mask);
 			}
@@ -707,7 +709,7 @@ void Sched_MC_mTask(void) {
 			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
 				mc_process_throttle_epm();
 			}else {
-				torque_manager(runMode, get_throttle_float());
+				throttle_process(runMode, get_throttle_float());
 			}
 			PMSM_FOC_Slow_Task();
 		}

Applications/foc/samples.c

@@ -39,7 +39,7 @@ void samples_init(void){
 #ifdef THROTTLE_CHAN
 	_throttle.filted_value = (0);
 	_throttle.value = (0);
-	_throttle.lowpass = (0.01f); 
+	_throttle.lowpass = (0.001f); 
 	sample_throttle();
 #endif
 #ifdef U_VOL_ADC_CHAN

Applications/prot/can_foc_msg.c

@@ -123,7 +123,7 @@ void can_report_plot_values(u8 can) {
 	u8 data[10];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Plot));
 	encode_float(data + 2, eCtrl_get_RefTorque());
-	encode_float(data + 6, PMSM_FOC_Get()->in.s_targetTorque);
+	encode_float(data + 6, 0);//PMSM_FOC_Get()->in.s_targetTorque);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }