加入电流刹车模式

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -271,6 +271,8 @@ u8 PMSM_FOC_CtrlMode(void) {
 		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_SPD;
 	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT) {
 		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT;
+	}else if (_gFOC_Ctrl.in.n_ctlMode == CTRL_MODE_CURRENT_BRK) {
+		_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_CURRENT_BRK;
 	}else {
 		if (!_gFOC_Ctrl.in.b_cruiseEna) {
 			_gFOC_Ctrl.out.n_RunMode = CTRL_MODE_TRQ;
@@ -281,14 +283,18 @@ u8 PMSM_FOC_CtrlMode(void) {
 			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
 		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, _gFOC_Ctrl.in.s_targetTorque);
-		}
+		}else if ((preMode == CTRL_MODE_CURRENT) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
+			PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, _gFOC_Ctrl.in.s_targetTorque);
+		}else if ((preMode == CTRL_MODE_TRQ) && (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT)) {
+			
+		}	
 	}
 	return _gFOC_Ctrl.out.n_RunMode;
 }
 
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
 static __INLINE void PMSM_FOC_idq_Assign(void) {
-	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
+	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || _gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
 		if (_gFOC_Ctrl.in.b_MTPA_calibrate && (_gFOC_Ctrl.in.s_manualAngle != INVALID_ANGLE)) {
 			float s, c;
 			normal_sincosf(degree_2_pi(_gFOC_Ctrl.in.s_manualAngle + 90.0f), &s, &c);
@@ -308,14 +314,21 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
-	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT) {
+	if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT || _gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
 		_gFOC_Ctrl.in.s_targetCurrent = eCtrl_get_RefCurrent();
+		if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_CURRENT_BRK) {
+			if (eCtrl_get_FinalCurrent() < 0.0001f && _gFOC_Ctrl.in.s_motRPM < 100) {
+				_gFOC_Ctrl.in.s_targetCurrent = 0.0f;
+				eCtrl_enable_eBrake(false);
+			}
+		}
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = eCtrl_get_RefTorque();
 		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
 		_gFOC_Ctrl.pi_ctl_trq->min = -refTorque;
-		if ((eCtrl_get_FinalTorque() <= 0.0001f) && _gFOC_Ctrl.in.s_motRPM < 10) {
-			_gFOC_Ctrl.pi_ctl_spd->min = 0; //防止倒转
+		if ((eCtrl_get_FinalTorque() <= 0.0001f) && _gFOC_Ctrl.in.s_motRPM < 100) {
+			_gFOC_Ctrl.pi_ctl_trq->max = 0;
+			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
 		}
 		float errRef = _gFOC_Ctrl.params.s_maxRPM - _gFOC_Ctrl.in.s_motRPM;
 		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_trq, errRef);

Applications/foc/core/PMSM_FOC_Core.h

@@ -114,6 +114,7 @@ typedef enum {
 #define CTRL_MODE_SPD                       ((u8)1U)
 #define CTRL_MODE_TRQ                       ((u8)2U)
 #define CTRL_MODE_CURRENT                   ((u8)3U)
+#define CTRL_MODE_CURRENT_BRK               ((u8)4U)
 
 #define FOC_CALIMOD_HALL               ((u8) 1U)
 #define FOC_CALIMOD_MTPA               ((u8) 2U)

Applications/foc/core/e_ctrl.c

@@ -1,6 +1,8 @@
 #include "e_ctrl.h"
 #include "foc/foc_config.h"
 #include "math/fix_math.h"
+#include "foc/core/PMSM_FOC_Core.h"
+
 #include "libs/logger.h"
 
 e_Ctrl g_eCtrl;
@@ -8,24 +10,26 @@ static void _eCtrl_set_TgtCurrent(float c);
 static void _eCtrl_set_TgtSpeed(float s);
 static void _eCtrl_set_TgtTorque(float t);
 
-void eCtrl_init(u16 ebrk_time, u16 accl_time){
-	g_eCtrl.ebrk_time = ebrk_time;
+void eCtrl_init(u16 accl_time, u16 dec_time){
+	g_eCtrl.dec_time = dec_time;
 	g_eCtrl.accl_time = accl_time;
 	if (g_eCtrl.accl_time == 0) {
 		g_eCtrl.accl_time = DEFAULT_D_TIME;
 	}
-	if (g_eCtrl.ebrk_time == 0) {
-		g_eCtrl.ebrk_time = DEFAULT_D_TIME;
+	if (g_eCtrl.dec_time == 0) {
+		g_eCtrl.dec_time = DEFAULT_D_TIME;
 	}
+	g_eCtrl.ebrk_time = 2;
+	
 	g_eCtrl.ebrk_shadow = g_eCtrl.ebrk_time;
 	g_eCtrl.accl_shadow = g_eCtrl.accl_time;
-
+	g_eCtrl.dec_shadow = g_eCtrl.dec_time;
 	eRamp_init(&g_eCtrl.current);
 	eRamp_init(&g_eCtrl.speed);
+	eRamp_init(&g_eCtrl.torque);
 }
 
-void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time) {
-	g_eCtrl.accl_shadow = accl_time;
+void eCtrl_set_ebrk_time(u16 ebrk_time) {
 	g_eCtrl.ebrk_shadow = ebrk_time;
 }
 
@@ -42,10 +46,31 @@ void eCtrl_set_TgtSpeed(float s) {
 	g_eCtrl.speed_shadow = s;
 }
 
+void eCtrl_enable_eBrake(bool enable) {
+	if (enable && PMSM_FOC_GetSpeed() > 1000.0f) {
+		g_eCtrl.is_ebrake_shadow = true;
+	}else if (!enable){
+		g_eCtrl.is_ebrake_shadow = false;
+	}
+}
+
+void _eCtrl_process_eBrake(void) {
+	if (g_eCtrl.is_ebrake) {
+		eRamp_init(&g_eCtrl.current);
+		eRamp_init(&g_eCtrl.speed);
+		eRamp_init(&g_eCtrl.torque);
+		PMSM_FOC_SetCtrlMode(CTRL_MODE_CURRENT_BRK);
+		eCtrl_set_TgtCurrent(-2.0f);
+	}else {
+		PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
+	}
+}
+
+
 void eCtrl_Running(void) {
 	bool etime_changed = false;
-	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.ebrk_shadow != g_eCtrl.ebrk_time) {
-		g_eCtrl.ebrk_time = g_eCtrl.ebrk_shadow;
+	if (g_eCtrl.accl_shadow != g_eCtrl.accl_time || g_eCtrl.dec_shadow != g_eCtrl.dec_time) {
+		g_eCtrl.dec_time = g_eCtrl.dec_shadow;
 		g_eCtrl.accl_time = g_eCtrl.accl_shadow;
 		etime_changed = true;
 	}
@@ -57,7 +82,11 @@ void eCtrl_Running(void) {
 	}
 	if (g_eCtrl.torque_shadow != g_eCtrl.torque.target || etime_changed) {
 		_eCtrl_set_TgtTorque(g_eCtrl.torque_shadow);
-	}	
+	}
+	if (g_eCtrl.is_ebrake_shadow != g_eCtrl.is_ebrake) {
+		g_eCtrl.is_ebrake = g_eCtrl.is_ebrake_shadow;
+		_eCtrl_process_eBrake();
+	}
 	eRamp_running(&g_eCtrl.current);
 	eRamp_running(&g_eCtrl.speed);
 	eRamp_running(&g_eCtrl.torque);
@@ -78,7 +107,7 @@ static void _eCtrl_set_target(e_Ramp *ramp, float c) {
 			step_val = MIN_S16Q5;
 		}
 	}else if (sign < 0) {
-		step_val = (c_now - c)/(g_eCtrl.ebrk_time/step_ms);
+		step_val = (c_now - c)/(g_eCtrl.dec_time/step_ms);
 		if (step_val < MIN_S16Q5) {
 			step_val = MIN_S16Q5;
 		}
@@ -86,8 +115,8 @@ static void _eCtrl_set_target(e_Ramp *ramp, float c) {
 	}
 	eRamp_set_target(ramp, c);
 	eRamp_set_step(ramp, step_val);
-
 }
+
 static void _eCtrl_set_TgtCurrent(float c) {
 	_eCtrl_set_target(&g_eCtrl.current, c);
 }

Applications/foc/core/e_ctrl.h

@@ -13,16 +13,21 @@ typedef struct {
 typedef struct {
 	u16  ebrk_time; //能量回收，时间越短，刹车性能或者回收越好
 	u16  accl_time; //加速时间（ms），时间越短，加速性能越好
+	u16  dec_time;  //降速时间
 	bool hw_brake;
+	bool is_ebrake;
 	u64  brake_ts;//检测到刹车开始时间
 	e_Ramp current;
 	e_Ramp torque;
 	e_Ramp speed;
 	u16  ebrk_shadow;
 	u16  accl_shadow;
+	u16  dec_shadow;
+	float ebrake_current;
 	float current_shadow;
 	float torque_shadow;
 	float speed_shadow;
+	bool is_ebrake_shadow;
 }e_Ctrl;
 static void eRamp_init(e_Ramp *r) {
 	r->start = 0;
@@ -59,12 +64,13 @@ static float eRamp_get_target(e_Ramp *r) {
 	return r->target;
 }
 
-void eCtrl_init(u16 ebrk_time, u16 accl_time);
-void eCtrl_set_accl_brk(u16 accl_time, u16 ebrk_time);
+void eCtrl_init(u16 accl_time, u16 dec_time);
+void eCtrl_set_ebrk_time(u16 ebrk_time);
 void eCtrl_brake_signal(bool hw_brake);
 void eCtrl_set_TgtCurrent(float c);
 void eCtrl_set_TgtTorque(float t);
 void eCtrl_set_TgtSpeed(float s);
+void eCtrl_enable_eBrake(bool enable);
 float eCtrl_get_RefSpeed(void);
 float eCtrl_get_RefCurrent(void);
 float eCtrl_get_RefTorque(void);

Applications/foc/core/torque.c

@@ -1,5 +1,9 @@
 #include "foc/core/torque.h"
 #include "foc/foc_config.h"
+#include "foc/motor/motor.h"
+#include "foc/core/e_ctrl.h"
+#include "foc/core/PMSM_FOC_Core.h"
+
 /*
 通过查表获取对应扭矩和速度时的Id和IQ的分配
 */
@@ -36,3 +40,18 @@ float torque_target_from_throttle(float f_throttle) {
 }
 
 
+void torque_speed_target(u8 run_mode, float f_throttle) {
+	if (run_mode == CTRL_MODE_SPD) {
+		float speed_Ref = speed_target_from_throttle(f_throttle);
+		PMSM_FOC_Set_Speed(speed_Ref);
+	}else if (run_mode == CTRL_MODE_TRQ) {
+		if (mc_throttle_released()) {
+			eCtrl_enable_eBrake(true);
+		}else {
+			float torque = torque_target_from_throttle(f_throttle);
+			eCtrl_enable_eBrake(false);
+			PMSM_FOC_Set_Torque(torque);
+		}
+	}
+}
+

Applications/foc/core/torque.h

@@ -7,6 +7,7 @@ void torque_init(void);
 void torque_get_idq(float torque, float rpm, DQ_t *dq_out);
 float speed_target_from_throttle(float f_throttle);
 float torque_target_from_throttle(float f_throttle);
+void torque_speed_target(u8 run_mode, float f_throttle);
 
 #endif /*_TORQUE_LUT_H__ */
 

Applications/foc/motor/motor.c

@@ -53,7 +53,7 @@ bool mc_start(u8 mode) {
 		err_add_record(FOC_Throttle_Err, get_throttle_float() * 10);
 		return false;
 	}
-	eCtrl_init(1000, 2000);
+	eCtrl_init(2000, 100);
 	motor_encoder_start(motor.s_direction);
 	PMSM_FOC_Start(mode);
 	pwm_turn_on_low_side();
@@ -223,13 +223,7 @@ void Sched_MC_mTask(void) {
 		float f_throttle = get_throttle_float();
 		if (f_throttle != motor.throttle) {
 			motor.throttle = f_throttle;
-			if (runMode == CTRL_MODE_SPD) {
-				float speed_Ref = speed_target_from_throttle(f_throttle);
-				PMSM_FOC_Set_Speed(speed_Ref);
-			}else if (runMode == CTRL_MODE_TRQ) {
-				float torque = torque_target_from_throttle(f_throttle);
-				PMSM_FOC_Set_Torque(torque);
-			}
+			torque_speed_target(runMode, f_throttle);
 		}		
 		PMSM_FOC_idqCalc();
 	}