转把处理

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

Applications/app/app.c

@@ -104,7 +104,7 @@ static u32 _app_report_task(void *p) {
 }
 
 static u32 _app_plot_task(void * args) {
-	can_report_plot_values(0x45);
+	//can_report_plot_values(0x45);
 	return 10;
 }
 static u32 _app_low_task(void *args) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -77,13 +77,6 @@ static __INLINE void FOC_Set_DqRamp(dq_Rctrl *c, float target, int time) {
 	float cp = c->s_Cp;
 	c->s_FinalTgt = target; 
 	c->s_Step = (c->s_FinalTgt - cp) / (float)time;
-	if ((c->s_Step == 0) && ((c->s_FinalTgt - cp) != 0.0f)) {
-		if (c->s_FinalTgt - cp > 0) {
-			c->s_Step = 0.001;	
-		}else if (c->s_FinalTgt - cp < 0){
-			c->s_Step = -0.001;
-		}
-	}
 }
 
 static __INLINE float FOC_Get_DqRamp(dq_Rctrl *c) {
@@ -431,8 +424,9 @@ u8 PMSM_FOC_CtrlMode(void) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_speed, target_troque);
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
-		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {
-			
+		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK)) {
+			eCtrl_reset_Current(min(PMSM_FOC_Get_Real_Torque(), gFoc_Ctrl.in.s_targetTorque));
+			eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
 		}	
 	}
 	
@@ -469,9 +463,10 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 	}else if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) || (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 		torque_get_idq(gFoc_Ctrl.in.s_targetTorque, gFoc_Ctrl.in.s_motRPM, &gFoc_Ctrl.in.s_targetIdq);
 	}
-
+	u32 mask = cpu_enter_critical();
 	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[0], gFoc_Ctrl.in.s_targetIdq.d);
 	FOC_Set_iDqRamp(&gFoc_Ctrl.idq_ctl[1], gFoc_Ctrl.in.s_targetIdq.q);
+	cpu_exit_critical(mask);
 }
 
 /*called in media task */

Applications/foc/core/e_ctrl.c

@@ -46,9 +46,15 @@ void eCtrl_set_TgtTorque(float t) {
 }
 
 void eCtrl_reset_Torque(float init_trq) {
+	g_eCtrl.torque_shadow = init_trq;
 	eRamp_reset_target(&g_eCtrl.torque, init_trq);
 }
 
+void eCtrl_reset_Current(float init_curr) {
+	g_eCtrl.current_shadow = init_curr;
+	eRamp_reset_target(&g_eCtrl.current, init_curr);
+}
+
 void eCtrl_set_TgtSpeed(float s) {
 	g_eCtrl.speed_shadow = s;
 }
@@ -76,11 +82,10 @@ static void _eCtrl_clear_ramp(void) {
 }
 
 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, min(PMSM_FOC_Get_Real_Torque() * 0.9f, eCtrl_get_RefTorque()));
-		eCtrl_set_TgtCurrent(-PMSM_FOC_GeteBrkPhaseCurrent());
+		eRamp_init(&g_eCtrl.current, nv_get_foc_params()->n_ebrk_time, nv_get_foc_params()->n_ebrk_time);
+		g_eCtrl.current_shadow = 0.0f;
 	}else {
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
 	}

Applications/foc/core/e_ctrl.h

@@ -37,6 +37,7 @@ typedef struct {
 	float speed_shadow;
 	bool is_ebrake_shadow;
 }e_Ctrl;
+
 static void eRamp_init(e_Ramp *r, u32 acc, u32 dec) {
 	r->start = 0;
 	r->target = 0;
@@ -124,11 +125,8 @@ static void eRamp_set_step_target(e_Ramp *ramp, float c, u32 intval) {
 }
 
 static void eRamp_X2_running(e_Ramp *r) {
-#if 1
-	if(r->target == r->interpolation) {
-		return;
-	}
-	if ((r->first_target != 0) && (r->interpolation != r->first_target)) {
+#if 0
+	if (r->first_target != 0) {
 		r->interpolation += r->step_val;
 		if (r->step_val > 0) {
 			if (r->interpolation >= r->first_target) {
@@ -141,9 +139,11 @@ static void eRamp_X2_running(e_Ramp *r) {
 				r->first_target = 0.0f;
 			}
 		}
+	}
+	if (r->first_target != 0) {
 		return;
 	}
-	if(r->time < 0xFFFFu) {
+	if(r->time < 0xFFFFFFFu) {
 		r->time ++;
 	}
 	r->interpolation = r->start + r->A * (float)SQ(r->time);
@@ -158,22 +158,24 @@ static void eRamp_X2_running(e_Ramp *r) {
 }
 
 static void eRamp_set_X2_target(e_Ramp *ramp, float c) {
-#if 1
+#if 0
 	float c_now = eRamp_get_intepolation(ramp);
 
 	float delta = c - c_now;
+	float first_delta = 0;
 	if (delta > 0) {
-		ramp->first_target = min(delta, 10.0f);
-		ramp->step_val = 0.02f;
-		ramp->A = (delta - ramp->first_target)/SQ(ramp->acct);
+		first_delta = 0;//min(delta, 20.0f);
+		ramp->first_target = 0;//c_now + first_delta;
+		ramp->step_val = 0.05f;
+		ramp->A = (delta - first_delta)/SQ(ramp->acct);
 	}else {
-		ramp->first_target = MAX(delta, -5.0f);
+		first_delta = 0;//MAX(delta, -10.0f);
+		ramp->first_target = 0;//c_now + first_delta;
 		ramp->step_val = -0.01f;
-		ramp->A = (delta - ramp->first_target)/SQ(ramp->dect);
+		ramp->A = (delta - first_delta)/SQ(ramp->dect);
 	}
-	ramp->start = c_now + ramp->first_target;
+	ramp->start = c_now + first_delta;
 	ramp->time = 0;
-
 	eRamp_set_target(ramp, c);
 #else
 	eRamp_set_step_target(ramp, c, CONFIG_eCTRL_STEP_TS);
@@ -198,6 +200,7 @@ float eCtrl_get_FinalTorque(void);
 void eCtrl_Running(void);
 void eCtrl_Reset(void);
 void eCtrl_reset_Torque(float init_trq);
+void eCtrl_reset_Current(float init_curr);
 
 #endif /* EBRAKE_CTRL_H__ */
 

Applications/foc/core/torque.c

@@ -57,9 +57,9 @@ static float throttle_ration(float f_throttle) {
 	}
 	float delta = f_throttle - (nv_get_foc_params()->n_minThroVol);
 
-	int ration = (delta * 100.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
+	int ration = (delta * 1000.0f) / (nv_get_foc_params()->n_maxThroVol - nv_get_foc_params()->n_minThroVol);
 	
-	return ((float)ration)/100.0f;
+	return ((float)ration)/1000.0f;
 }
 
 float throttle_to_speed(float f_throttle) {
@@ -70,62 +70,109 @@ float throttle_to_torque(float f_throttle) {
 	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);
+#define REAL_DQ_CEOF 1.1f
 
-	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;
-		}
+#if 1
+void torque_mode_process(void) {
+	float ref_trq = PMSM_FOC_GetTorqueLimit() * g_trq_mn.thro_value;
+
+	if ((mc_throttle_released()) && eCtrl_enable_eBrake(true)) {
+		g_trq_mn.thro_value = 0;
 		return;
 	}
 
-	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();
+	if (!mc_throttle_released()) {
+		if (PMSM_FOC_GetSpeed() <= 1.0f) {
+			ref_trq  = MAX(eCtrl_get_FinalTorque() * REAL_DQ_CEOF, ref_trq );
 		}
-		trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);		
+		PMSM_FOC_Set_Torque(ref_trq );
+	}else if (mc_throttle_released() && eCtrl_get_FinalTorque() != 0){
+		float real_trq = PMSM_FOC_Get_Real_Torque();
+		float ref_now = min(real_trq, eCtrl_get_RefTorque());
+		eCtrl_reset_Torque(ref_now);
+		PMSM_FOC_Set_Torque(0);
+		g_trq_mn.thro_value = 0;
+	}
+}
+#else
+void torque_mode_process(void) {
+	float thro_curr = g_trq_mn.thro_value;
+	float thro_prev = g_trq_mn.thro_prev;
+	float trq_ref = PMSM_FOC_GetTorqueLimit() * thro_curr;
 
-		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;
+	if ((thro_curr == 0.0f) && eCtrl_enable_eBrake(true)) {
+		g_trq_mn.thro_value = 0;
 		g_trq_mn.accl = false;
-		
+		g_trq_mn.thro_prev = 0.0f;
+		return;
+	}
+	float real_trq = eCtrl_get_FinalTorque() * REAL_DQ_CEOF;
+	if (trq_ref > 0) {
+		if (PMSM_FOC_GetSpeed() <= 10.0f) {
+			g_trq_mn.accl_ref = MAX(real_trq, trq_ref); //不能小于autohold产生的扭矩
+			trq_ref = g_trq_mn.accl_ref;
+		}else {
+			if (thro_curr > thro_prev) {
+				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(real_trq, 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;
+			}else {
+				if (g_trq_mn.accl) {
+					trq_ref = g_trq_mn.accl_ref + thro_curr * (PMSM_FOC_GetTorqueLimit() - g_trq_mn.accl_ref);
+				}else {
+					trq_ref = thro_curr * g_trq_mn.accl_ref;
+				}
+			}
+		}
+		PMSM_FOC_Set_Torque(trq_ref);
+	}
+	else if (eCtrl_get_FinalTorque() != 0){
+		float ref_now = min(real_trq, eCtrl_get_RefTorque());
+		eCtrl_reset_Torque(ref_now);
+		PMSM_FOC_Set_Torque(0);
+		g_trq_mn.thro_value = 0;
 	}
-	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);
+#endif
+void speed_mode_process(void) {
+	float speed_Ref = g_trq_mn.spd_ref;
 	PMSM_FOC_Set_Speed(speed_Ref);
 }
 
+#define THRO_REF_LP_CEOF 0.2f
+
 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);
+		float thro_value = throttle_ration(f_throttle);
+		g_trq_mn.thro_value = LowPass_Filter(g_trq_mn.thro_value, thro_value, THRO_REF_LP_CEOF);
+		if ((g_trq_mn.count++ % 20) == 0) {
+			torque_mode_process();
+		}
+	
}else if (run_mode == CTRL_MODE_SPD) {
+		float spd_ref = throttle_to_speed(f_throttle);
+		g_trq_mn.spd_ref = LowPass_Filter(g_trq_mn.spd_ref, spd_ref, THRO_REF_LP_CEOF);
+		if ((g_trq_mn.count++ % 20) == 0) {
+			speed_mode_process();
+		}
 	}else if (run_mode == CTRL_MODE_CURRENT_BRK) {
+		eCtrl_reset_Torque(0);
+		if (eCtrl_get_FinalCurrent() < 0.0001f && PMSM_FOC_GetSpeed() < CONFIG_MIN_RPM_EXIT_EBRAKE) {
+			eCtrl_enable_eBrake(false);	
+		}
 		if (!mc_throttle_released() || (mc_throttle_released() && (PMSM_FOC_GetSpeed() == 0.0f))) {
 			eCtrl_enable_eBrake(false);
 		}

Applications/foc/core/torque.h

@@ -6,13 +6,10 @@ typedef struct {
 	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;
+	bool  accl;
+	float thro_value; //油门开度百分比
+	float spd_ref;
 	u32   count;
 }torque_manager_t;
 void torque_init(void);

Applications/foc/motor/motor.c

@@ -73,6 +73,7 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.epm_dir = EPM_Dir_None;
 	motor.n_autohold_time = 0;
 	motor.b_auto_hold = 0;
+	motor.b_break = false;
 }
 void mc_init(void) {
 	adc_init();
@@ -488,14 +489,18 @@ static bool mc_is_hwbrake(void) {
 }
 void MC_Brake_IRQHandler(void) {
 
+	if (mc_is_hwbrake()) {
+		motor.b_break = true;
+	}else {
+		motor.b_break = false;
+	}
+
 	if (!motor.b_start) {
 		return;
 	}
-	if (mc_is_hwbrake()) {
-		motor.b_break = true;
+	if (motor.b_break) {
 		PMSM_FOC_Brake(true);
 	}else {
-		motor.b_break = false;
 		PMSM_FOC_Brake(false);
 	}
 }
@@ -590,7 +595,7 @@ static bool mc_run_stall_process(u8 run_mode) {
 			}
 			motor.runStall_time = 0;
 			motor.b_runStall = false; //转把释放，清除堵转标志
-		}else if (PMSM_FOC_Get()->out.s_RealIdq.q >= CONFIG_STALL_MAX_CURRENT){
+		}else if (PMSM_FOC_Get_Real_Torque() >= CONFIG_STALL_MAX_CURRENT){
 			if (ABS(PMSM_FOC_GetSpeed()) < 1.0f && (motor.runStall_time == 0)) {
 				motor.runStall_time = get_tick_ms();
 				motor.runStall_pos = motor_encoder_get_position();
@@ -599,7 +604,8 @@ 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;
-					throttle_process(run_mode, 0.0f);
+					PMSM_FOC_Set_Torque(0);
+					torque_reset();
 					return true;
 				}
 				if (ABS(motor.runStall_pos - motor_encoder_get_position()) >= 0.2f) {

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.001f); 
+	_throttle.lowpass = (0.01f); 
 	sample_throttle();
 #endif
 #ifdef U_VOL_ADC_CHAN