加入EPM 模式

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

Applications/app/key_process.c

@@ -38,8 +38,9 @@ static u32 key_task(void *p) {
 	value = gpio_stopkey_value();
 	if (value != key_value[KEY_STOP]) {
 		if (value) {
-			//PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
-			PMSM_FOC_LockMotor(true);
+			PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
+			//PMSM_FOC_SpeedLimit(100);
+			//PMSM_FOC_LockMotor(true);
 		}
 		key_value[KEY_STOP] = value;
 	}

Applications/app/nv_storage.c

@@ -32,7 +32,7 @@ static void nv_default_motor_params(void) {
 	m_params.ld = MOTOR_Ld;
 	m_params.lq = MOTOR_Lq;
 	m_params.encoder_offset = -50;
-	m_params.est_pll_band = 200;
+	m_params.est_pll_band = 20;
 	m_params.pos_lock_pll_band = 200;
 	m_params.flux_linkage = 0.0f;
 	m_params.hall_offset = 0;
@@ -44,6 +44,7 @@ static void nv_default_foc_params(void) {
 	foc_params.s_maxIdq = 20;
 	foc_params.s_minIdq = -20;
 	foc_params.s_maxRPM = 8200;
+	foc_params.s_maxEpmRPM = 133;
 	foc_params.s_maxTorque = 15;
 	foc_params.s_maxBrkCurrent = 2.0f;
 	foc_params.n_currentBand = 500;

Applications/app/nv_storage.h

@@ -10,6 +10,7 @@ typedef struct {
 	float s_maxiDC;
 	float s_maxvDC;
 	float s_maxRPM;
+	float s_maxEpmRPM;
 	float s_maxTorque;
 	float s_maxBrkCurrent;
 	float n_modulation;

Applications/foc/commands.c

@@ -97,6 +97,15 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			memcpy((char *)pid, (char *)command->data, 5 * sizeof(pid_conf_t));
 			conf_foc_pid(pid);
 		}
+		case Foc_Start_EPM_Move:
+		{
+			bool move = decode_u8((u8 *)command->data) == 0?false:true;
+			EPM_Dir_t dir = (EPM_Dir_t)decode_u8((u8 *)command->data + 1);
+			if(!PMSM_FOC_Start_epmMove(move, dir)) {
+				erroCode = PMSM_FOC_GetErrCode();
+			}
+			break;
+		}
 		default:
 		{
 			erroCode = FOC_Unknow_Cmd;

Applications/foc/commands.h

@@ -23,6 +23,7 @@ typedef enum {
 	Foc_Report_Phase_Vol,		//u32,u32,u32
 	Foc_Report_Dq_Vol,			//u32, u32
 	Foc_Set_Open_Dq_Vol,  		//u32, u32, 114d000000000a000000, 114d0000000000000000
+	Foc_Start_EPM_Move,
 	Foc_Cmd_Max
 }foc_cmd_t;
 #define CMD_2_CAN_KEY(cmd) ((u16)(((u16)cmd) | (CAN_MY_ADDRESS<<8)))

Applications/foc/core/PMSM_FOC_Core.c

@@ -201,7 +201,7 @@ static __INLINE void PMSM_FOC_Plot_Debug(void) {
 		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
 		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), _gFOC_Ctrl.out.test_sample*100);
 		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(encoder_get_pwm_angle()));
-		plot_1data16(motor_encoder_get_vel_count() * 10);
+		plot_1data16(_gFOC_Ctrl.in.s_motRPM);
 	}
 }
 
@@ -443,10 +443,53 @@ bool PMSM_FOC_Set_Speed(float rpm) {
 	if (_gFOC_Ctrl.in.b_cruiseEna) {
 		return false;
 	}
-	eCtrl_set_TgtSpeed(min(rpm, _gFOC_Ctrl.params.s_maxRPM));
+	eCtrl_set_TgtSpeed(min(ABS(rpm), _gFOC_Ctrl.params.s_maxRPM)*SIGN(rpm));
 	return true;
 }
 
+bool PMSM_FOC_Set_epmMode(bool epm) {
+	if (_gFOC_Ctrl.in.b_epmMode != epm) {
+		if (PMSM_FOC_GetSpeed() != 0.0f) {
+			PMSM_FOC_SetErrCode(FOC_NowAllowed_With_Speed);
+			return false;
+		}
+		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
+		_gFOC_Ctrl.in.b_epmMode = epm;
+		if (epm) {
+			PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxEpmRPM);
+			eCtrl_set_TgtSpeed(0);
+			PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
+		}else {
+			PMSM_FOC_SetCtrlMode(CTRL_MODE_TRQ);
+			PMSM_FOC_SpeedLimit(nv_get_foc_params()->s_maxRPM);
+		}
+	}
+	return true;
+}
+
+bool PMSM_FOC_is_epmMode(void) {
+	return _gFOC_Ctrl.in.b_epmMode;
+}
+
+bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir) {
+	if (!_gFOC_Ctrl.in.b_epmMode) {
+		return false;
+	}
+	if (move) {
+		if (_gFOC_Ctrl.in.epmDirection != EPM_Dir_None) {
+			return false;
+		}
+		_gFOC_Ctrl.in.epmDirection = dir;
+	}else {
+		_gFOC_Ctrl.in.epmDirection = EPM_Dir_None;
+	}
+	return true;
+}
+
+EPM_Dir_t PMSM_FOC_Get_epmDir(void) {
+	return _gFOC_Ctrl.in.epmDirection;
+}
+
 bool PMSM_FOC_Set_Current(float is) {
 	eCtrl_set_TgtCurrent(is);
 	return true;

Applications/foc/core/PMSM_FOC_Core.h

@@ -12,6 +12,13 @@ typedef struct {
 	float q;
 }DQ_t;
 
+typedef enum {
+	EPM_Dir_None,
+	EPM_Dir_Back,
+	EPM_Dir_Forward,
+}EPM_Dir_t;
+
+
 typedef struct {
 	u8 	  n_poles;
 	float s_maxIdq;
@@ -39,11 +46,13 @@ typedef struct {
 	DQ_t    s_targetVdq;
 	float   s_targetTorque; //限速后的实际扭矩
 	float 	s_vDC;
+	EPM_Dir_t epmDirection;
 	u8      n_ctlMode;
 	bool    b_motEnable;
 	bool    b_cruiseEna;
 	bool    b_motLock;
 	bool    b_eBrake;
+	bool    b_epmMode;
 	bool    b_MTPA_calibrate;
 	float   s_manualAngle; //mainly used when calibrate hall/mtpa. 
 }FOC_InP;
@@ -173,6 +182,10 @@ void PMSM_FOC_MTPA_Calibrate(bool enable);
 void PMSM_FOC_Get_TgtIDQ(DQ_t * dq);
 void PMSM_FOC_TorqueLimit(float torqueLimit);
 float PMSM_FOC_GetTorqueLimit(void);
+bool PMSM_FOC_Set_epmMode(bool epm);
+bool PMSM_FOC_is_epmMode(void);
+bool PMSM_FOC_Start_epmMove(bool move, EPM_Dir_t dir);
+EPM_Dir_t PMSM_FOC_Get_epmDir(void);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/core/torque.c

@@ -43,6 +43,13 @@ 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);
+		if (PMSM_FOC_is_epmMode()) {
+			if (speed_Ref == 0.0f) {
+				PMSM_FOC_Start_epmMove(false, EPM_Dir_None);
+			}else {
+				PMSM_FOC_Start_epmMove(true, EPM_Dir_Forward);
+			}
+		}
 		PMSM_FOC_Set_Speed(speed_Ref);
 	}else if (run_mode == CTRL_MODE_TRQ) {
 		if (mc_throttle_released()) {

Applications/math/fast_math.h

@@ -35,6 +35,7 @@ static void normal_sincosf(float angle, float *sin, float *cos) {
 
 #define INVALID_ANGLE 0x3DFF
 
+#define SIGN(x)				(((x) < 0.0f) ? -1.0f : 1.0f)
 
 
 /**