1. 加速EPM后退的扭矩限制和最大速度限制

2. EPM模式区分前进后退，给不通的扭矩限制和最大速度

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

Applications/app/app.c

@@ -126,8 +126,8 @@ static u32 _app_report_task(void *p) {
 	can_report_phase_current(0x45);
 	if (++loop % 10 == 0) {
 		//sys_debug("rst 0x%x\n", get_mcu_reset_source());
-		//sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
-		//sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
+		sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+		sys_debug("Fast: %d - %d, err: %d-%d-%d\n", g_meas_foc.intval_time, g_meas_foc.exec_time, g_meas_foc.intval_hi_err, g_meas_foc.intval_low_err, g_meas_foc.exec_max_error_time);
 		//sys_debug("FOC time err %d %d\n", g_meas_foc.intval_time_h_error, g_meas_foc.intval_time_l_error);
 		//sys_debug("acc vol %d, bid %d\n", get_acc_vol(), gpio_board_id());
 		//sys_debug("throttle %f\n", get_throttle_float());
@@ -142,6 +142,7 @@ static u32 _app_report_task(void *p) {
 		PMSM_FOC_LogDebug();
 		//F_debug();
 		//eCtrl_debug_log();
+		sys_debug("=====\n");
 		//err_code_log();
 	}
 	return 200;

Applications/app/nv_storage.c

@@ -50,7 +50,7 @@ static void nv_default_motor_params(void) {
 #endif
 	m_params.offset = MOTOR_ENC_OFFSET;//180;//(69.0f);
 	m_params.est_pll_band = 50;
-	m_params.epm_pll_band = 150;
+	m_params.epm_pll_band = 400;
 	m_params.pos_lock_pll_band = 500;
 	m_params.velocity_weight = 190;
 	m_params.velocity_C = 145;
@@ -104,6 +104,10 @@ static void nv_default_foc_params(void) {
 	foc_params.n_FwEnable  = CONFIG_DEFAULT_FW_ENABLE;
 	foc_params.f_minThroVol = CONFIG_THROTTLE_MIN_VALUE;
 	foc_params.f_maxThroVol = CONFIG_THROTTLE_MAX_VALUE;
+
+	foc_params.s_maxEpmRPMBck = CONFIG_DEFAULT_EPM_BK_RPM;
+	foc_params.s_maxEpmTorqueLimBck = CONFIG_DEFAULT_EPM_BK_PHASE_CURR;
+
 	foc_params.pid_conf[PID_D_id].kp = (foc_params.n_currentBand * MOTOR_Ld);
 	foc_params.pid_conf[PID_D_id].ki = (MOTOR_R/MOTOR_Ld);
 	foc_params.pid_conf[PID_D_id].kd = 0;

Applications/app/nv_storage.h

@@ -36,7 +36,10 @@ typedef struct {
 	float f_adrc_vel_Wcv;
 	float f_adrc_vel_B0;
 
-	u8    res[256 - 157 - 24];
+	float s_maxEpmRPMBck;
+	float s_maxEpmTorqueLimBck;
+
+	u8    res[256 - 157 - 24 - 8];
 	u16   crc16;
 }foc_params_t;
 

Applications/foc/commands.c

@@ -61,9 +61,9 @@ static void process_ext_command(foc_cmd_body_t *command) {
 			mc_stop();
 		}
 		s8 ext_gear = decode_8bits(b0, 5, 7);
-		if (ext_gear >= 0 && ext_gear <= 5) {
-			sys_debug("gear %d\n", ext_gear);
-			if (ext_gear == 0) {
+		sys_debug("gear %d\n", ext_gear);
+		if (ext_gear >= 1 && ext_gear <= 4) {
+			if (ext_gear == 4) {
 				mc_set_gear(3);
 			}else {
 				mc_set_gear(ext_gear - 1);
@@ -490,6 +490,10 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			len += 4;
 			encode_u32(config+len, nv_get_foc_params()->n_ebrk_time);
 			len += 4;
+			encode_s16(config+len, nv_get_foc_params()->s_maxEpmRPMBck);
+			len += 2;
+			encode_s16(config+len, nv_get_foc_params()->s_maxEpmTorqueLimBck);
+			len += 2;
 			can_send_response(command->can_src, config, len);
 			os_free(config);
 			return;
@@ -498,7 +502,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		{
 			if (mc_is_start()) {
 				erroCode = FOC_NotAllowed;
-			}else if (command->len < 24) {
+			}else if (command->len < 28) {
 				erroCode = FOC_Param_Err;
 			}else {
 				nv_get_foc_params()->s_TorqueBrkLim = decode_s16((u8 *)command->data);
@@ -512,6 +516,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				nv_get_foc_params()->n_autoHold = decode_u8((u8 *)command->data + 15);
 				nv_get_foc_params()->n_dec_time = decode_u32((u8 *)command->data + 16);
 				nv_get_foc_params()->n_ebrk_time = decode_u32((u8 *)command->data + 20);
+				nv_get_foc_params()->s_maxEpmRPMBck = decode_s16((u8 *)command->data + 24);
+				nv_get_foc_params()->s_maxEpmTorqueLimBck = decode_s16((u8 *)command->data + 26);
 				nv_save_foc_params();
 				shark_timer_post(&_reboot_timer, 200);
 			}

Applications/foc/core/thro_torque.c

@@ -243,5 +243,5 @@ float get_user_request_torque(void) {
 
 void thro_torque_log(void) {
 	sys_debug("accl %d, real %f, req %f\n", _torque.accl, _torque.torque_real, _torque.torque_req);
-	sys_debug("ration %f - %f - %f\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate());
+	sys_debug("ration %f - %f - %f - %d\n", _torque.thro_ration, _torque.thro_ration_last, thro_torque_for_accelerate(), _torque.gear);
 }

Applications/foc/foc_config.h

@@ -7,7 +7,11 @@
 #define CONFIG_DEFAULT_RPM_LIM CONFIG_MAX_MOT_RPM
 
 #define CONFIG_DEFAULT_EPM_PHASE_CURR 100
-#define CONFIG_DEFAULT_EPM_RPM        300
+#define CONFIG_DEFAULT_EPM_RPM        200
+
+#define CONFIG_DEFAULT_EPM_BK_PHASE_CURR 50
+#define CONFIG_DEFAULT_EPM_BK_RPM        170
+
 #define CONFIG_DEFAULT_EBRK_TORQUE 0 //0:means disable ebrake
 #define CONFIG_DEFAULT_EBRK_IDC_LIM 15
 #define CONFIG_SVM_MODULATION       1.0f//(1.0F/SQRT3_BY_2)

Applications/foc/motor/motor.c

@@ -163,6 +163,8 @@ static void _mc_internal_init(u8 mode, bool start) {
 	motor.b_force_run = false;
 	motor.b_cruise = false;
 	motor.b_limit_pending = false;
+	motor.f_epm_trq = 0;
+	motor.f_epm_vel = 0;
 }
 
 static void _led_off_timer_handler(shark_timer_t *t) {
@@ -177,7 +179,6 @@ static void mc_gear_vmode_changed(void) {
 	PMSM_FOC_SpeedLimit(gears->n_max_speed);
 	PMSM_FOC_DCCurrLimit(min(gears->n_max_idc, motor.idc_user_lim));
 	PMSM_FOC_TorqueLimit(gears->n_max_trq);
-	//eCtrl_set_accl_time((u16)gears->n_accl_time); 放到转把处理的地方设置
 }
 
 static s16 mc_get_gear_idc_limit(void) {
@@ -360,8 +361,8 @@ bool mc_set_gear(u8 gear) {
 		return false;
 	}
 	if (motor.n_gear != gear) {
-		motor.n_gear = gear;
 		u32 mask = cpu_enter_critical();
+		motor.n_gear = gear;
 		mc_gear_vmode_changed();
 		cpu_exit_critical(mask);
 	}
@@ -467,11 +468,14 @@ bool mc_start_epm(bool epm) {
 	}
 	u32 mask = cpu_enter_critical();
 	motor.b_epm = epm;
+	motor.f_epm_vel = 0.0f;
+	motor.f_epm_trq = 0.0f;
 	motor_encoder_band_epm(epm);
 	if (epm) {
 		eCtrl_set_TgtSpeed(0);
 		motor.mode = CTRL_MODE_SPD;
-		PMSM_FOC_TorqueLimit(nv_get_foc_params()->s_maxEpmTorqueLim);
+		motor.epm_dir = EPM_Dir_None;
+		PMSM_FOC_TorqueLimit(nv_get_foc_params()->s_maxEpmTorqueLimBck);
 		PMSM_FOC_SetCtrlMode(CTRL_MODE_SPD);
 	}else {
 		motor.epm_dir = EPM_Dir_None;
@@ -502,6 +506,10 @@ bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
 		return true;
 	}
 	u32 mask = cpu_enter_critical();
+	if (motor.epm_dir != dir) {
+		motor.f_epm_vel = 0.0f;
+		motor.f_epm_trq = 0.0f;
+	}
 	motor.epm_dir = dir;
 	if (dir != EPM_Dir_None) {
 		motor.b_epm_cmd_move = is_command;
@@ -509,13 +517,11 @@ bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
 		if (!PMSM_FOC_Is_Start()) {
 			PMSM_FOC_Start(motor.mode);
 			pwm_enable_channel();
+		}else if (PMSM_FOC_AutoHoldding()) {
+			mc_auto_hold(false);
 		}
-		float rpm = nv_get_foc_params()->s_maxEpmRPM;
-		if (dir == EPM_Dir_Back) {
-			rpm = -rpm;
-		}
-		sys_debug("rpm %f\n", rpm);
-		PMSM_FOC_Set_Speed(rpm);
+		PMSM_FOC_TorqueLimit(motor.f_epm_trq);
+		PMSM_FOC_Set_Speed(motor.f_epm_vel);
 	}else {
 		motor.b_epm_cmd_move = false;
 		PMSM_FOC_Set_Speed(0);
@@ -1036,31 +1042,6 @@ void ADC_IRQHandler(void) {
 	TIME_MEATURE_END();
 }
 
-#ifndef CONFIG_DQ_STEP_RESPONSE
-static bool mc_can_stop_foc(void) {
-	if (mc_critical_can_not_run()) {
-		return true;
-	}
-	if (motor.mode == CTRL_MODE_CURRENT) {
-		return false;
-	}
-	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
-		if (!PMSM_FOC_AutoHoldding() && motor.epm_dir == EPM_Dir_None) {
-			return true;
-		}
-	}
-	/* 启用无感观测器，同时速度低于观测器允许的最小速度，关闭输出，滑行 */
-	if (!foc_observer_is_encoder() && PMSM_FOC_GetSpeed() < foc_observer_sensorless_working_speed()) {
-		return true;
-	}
-	return false;
-}
-
-static bool mc_can_restart_foc(void) {
-	return (!PMSM_FOC_Is_Start()) && (!mc_throttle_released()) && (!mc_unsafe_critical_error());
-}
-
-#endif
 
 static bool mc_run_stall_process(u8 run_mode) {
 	if ((run_mode == CTRL_MODE_TRQ || run_mode == CTRL_MODE_SPD) && !PMSM_FOC_AutoHoldding()) {
@@ -1142,6 +1123,24 @@ static void mc_process_throttle_epm(void) {
 	}
 }
 
+static void mc_process_epm_move(void) {
+	if (!motor.b_epm || (motor.epm_dir == EPM_Dir_None)){
+		return;
+	}
+	float target_vel = nv_get_foc_params()->s_maxEpmRPM;
+	float target_trq = nv_get_foc_params()->s_maxEpmTorqueLim;
+	float step = 0.2f;
+	if (motor.epm_dir == EPM_Dir_Back) {
+		target_vel = -nv_get_foc_params()->s_maxEpmRPMBck;
+		target_trq = nv_get_foc_params()->s_maxEpmTorqueLimBck;
+		step = 0.1f;
+	}
+	step_towards(&motor.f_epm_vel, target_vel, step);
+	step_towards(&motor.f_epm_trq, target_trq, 0.05f);
+	PMSM_FOC_TorqueLimit(motor.f_epm_trq);
+	PMSM_FOC_Set_Speed(motor.f_epm_vel);
+}
+
 static bool mc_process_force_running(void) {
 	if (motor.b_calibrate || (motor.mode == CTRL_MODE_OPEN)) {
 		if (motor.b_force_run) {
@@ -1196,6 +1195,32 @@ static void mc_process_curise(void) {
 }
 #endif
 
+#ifndef CONFIG_DQ_STEP_RESPONSE
+static bool mc_can_stop_foc(void) {
+	if (mc_critical_can_not_run()) {
+		return true;
+	}
+	if (motor.mode == CTRL_MODE_CURRENT) {
+		return false;
+	}
+	if (mc_throttle_released() && PMSM_FOC_GetSpeed() == 0.0f) {
+		if (!PMSM_FOC_AutoHoldding() && motor.epm_dir == EPM_Dir_None) {
+			return true;
+		}
+	}
+	/* 启用无感观测器，同时速度低于观测器允许的最小速度，关闭输出，滑行 */
+	if (!foc_observer_is_encoder() && PMSM_FOC_GetSpeed() < foc_observer_sensorless_working_speed()) {
+		return true;
+	}
+	return false;
+}
+
+static bool mc_can_restart_foc(void) {
+	return (!PMSM_FOC_Is_Start()) && (!mc_throttle_released() || (motor.epm_dir != EPM_Dir_None)) && (!mc_unsafe_critical_error());
+}
+
+#endif
+
 static void mc_motor_runstop(void) {
 	u32 mask;
 	if (mc_can_stop_foc()) {
@@ -1283,6 +1308,7 @@ void Sched_MC_mTask(void) {
 			eCtrl_Running();
 			if ((runMode == CTRL_MODE_SPD) && mc_is_epm()) {
 				mc_process_throttle_epm();
+				mc_process_epm_move();
 			}else {
 				float thro = get_throttle_float();
 				if (motor.b_ignor_throttle) {

Applications/foc/motor/motor.h

@@ -32,6 +32,8 @@ typedef struct {
 	float  cruise_torque;
 	EPM_Dir_t epm_dir;
 	bool   b_epm_cmd_move;
+	float  f_epm_vel;
+	float  f_epm_trq;
 	u32    runStall_time;
 	float  runStall_pos;
 	u8     u_throttle_ration;