挡位切换功能

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

Applications/app/app.c

@@ -96,9 +96,11 @@ static u32 _app_report_task(void *p) {
 	can_report_ext_status(0x43);
 	
 	if (++loop % 10 == 0) {
-		sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation);
-		encoder_log();
-		err_code_log();
+		//sys_debug("modulation %f, %f\n", PMSM_FOC_Get()->out.f_vdqRation, PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation);
+		sys_debug("Slow: %d - %d\n", g_meas_MCTask.intval_time, g_meas_MCTask.exec_time);
+		sys_debug("Fast: %d - %d, err: %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);
+		//encoder_log();
+		//err_code_log();
 	}
 	return 200;
 }
@@ -107,7 +109,8 @@ static u32 _app_report_task(void *p) {
 static u32 _app_plot_task(void * args) {
 	//can_report_plot_values(0x45);
 	//can_plot3(PMSM_FOC_Get()->out.n_Duty[0], PMSM_FOC_Get()->out.n_Duty[1], PMSM_FOC_Get()->out.n_Duty[2]);
-	can_plot2(PMSM_FOC_Get()->in.s_targetTorque * 10, PMSM_FOC_GetSpeed());
+	//can_plot2(PMSM_FOC_Get()->rtLim.rpmLimRamp.interpolation, PMSM_FOC_GetSpeed());
+	can_plot2(PMSM_FOC_Get()->in.s_targetTorque, PMSM_FOC_Get_Real_Torque()*SIGN(PMSM_FOC_Get()->out.s_FilterIdq.q));
 #if 0
 	if (!_mc_start) {
 		_mc_start = true;

Applications/app/nv_storage.c

@@ -63,16 +63,16 @@ static void nv_default_foc_params(void) {
 	foc_params.n_dec_time = CONFIG_DEC_TIME;
 	foc_params.n_ebrk_time = CONFIG_EBRK_RAMP_TIME;
 	foc_params.n_FwEnable  = CONFIG_DEFAULT_FW_ENABLE;
-	foc_params.pid_conf[PID_D_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Ld);
+	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].kb = 0;
 	
-	foc_params.pid_conf[PID_Q_id].kp = (CONFIG_CURRENT_BANDWITH * MOTOR_Lq);
+	foc_params.pid_conf[PID_Q_id].kp = (foc_params.n_currentBand * MOTOR_Lq);
 	foc_params.pid_conf[PID_Q_id].ki = (MOTOR_R/MOTOR_Lq);
 	foc_params.pid_conf[PID_Q_id].kb = 0;
 
-	foc_params.pid_conf[PID_TRQ_id].kp = 0.07f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.2f;
+	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.5f;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
 	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
@@ -291,5 +291,6 @@ void nv_storage_init(void) {
 		nv_save_foc_params();
 		nv_save_motor_params();
 	}
+	sys_debug("current band %f\n", foc_params.n_currentBand);
 }
 

Applications/bsp/board_mc_v1.h

@@ -17,7 +17,7 @@
 #define CONFIG_MAX_PHASE_VOL    (CONFIG_MOS_MAX_VOL - 20.0F)
 #define CONFIG_MAX_TORQUE       50.0F
 
-#define CONFIG_CURRENT_BANDWITH 1500.0f /* 电流环带宽 */
+#define CONFIG_CURRENT_BANDWITH  1000.0f /* 电流环带宽 */
 #define CONFIG_BEEP 
 #define CONFIG_STALL_MAX_CURRENT 100.0f //最大堵转相电流电流
 #define CONFIG_STALL_MAX_TIME    3000   //ms, 超过最大堵转电流持续时间，判断堵转

Applications/foc/commands.c

@@ -186,7 +186,6 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				PMSM_FOC_SpeedLimit((float)maxRPM);
 				PMSM_FOC_PhaseCurrLim((float)maxPhaseCurr);
 				PMSM_FOC_DCCurrLimit((float)maxiDC);
-				mc_need_update();//重新获取转把相电流和速度
 				encode_u16(response + 3, (u16)PMSM_FOC_GetSpeedLimit());
 				encode_u16(response + 5, (u16)PMSM_FOC_GetPhaseCurrLim());
 				encode_u8(response + 7, (u8)PMSM_FOC_GetDCCurrLimit());
@@ -235,6 +234,10 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		}
 		case Foc_Conf_Pid:
 		{
+			if (command->len < 13) {
+				erroCode = FOC_Param_Err;
+				break;
+			}
 			pid_conf_t pid;
 			u8 id =   decode_u8((u8 *)command->data);
 			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
@@ -243,6 +246,23 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			nv_set_pid(id, &pid);
 			break;
 		}
+		case Foc_Get_Pid:
+		{
+			pid_conf_t pid;
+			u8 id =   decode_u8((u8 *)command->data);
+			if (id < PID_Max_id) {
+				nv_get_pid(id, &pid);
+				erroCode = id;
+				memcpy(response+3, &pid, sizeof(pid));
+				len = sizeof(pid) + 3;
+				sys_debug("id = %d, kp = %f, ki = %f, kb = %f\n", id, pid.kp, pid.ki, pid.kb);
+			}else {
+				erroCode = 1;
+				len = 3;
+			}
+			break;
+		}
+
 		case Foc_Set_EPM_Mode:
 		{
 			bool mode = decode_u8((u8 *)command->data) == 0?false:true;

Applications/foc/commands.h

@@ -28,6 +28,7 @@ typedef enum {
 	Foc_Auto_Hold,
 	Foc_Set_Config,
 	Foc_Get_Config,
+	Foc_Get_Pid,
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,

Applications/foc/core/PMSM_FOC_Core.c

@@ -381,8 +381,8 @@ void PMSM_FOC_Schedule(void) {
 	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);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.d, gFoc_Ctrl.out.s_RealIdq.d, 0.004f);
+	LowPass_Filter(gFoc_Ctrl.out.s_FilterIdq.q, gFoc_Ctrl.out.s_RealIdq.q, 0.004f);
 #endif
 
 	if (gFoc_Ctrl.plot_type != Plot_None) {
@@ -442,6 +442,31 @@ u8 PMSM_FOC_CtrlMode(void) {
 	return gFoc_Ctrl.out.n_RunMode;
 }
 
+static void crosszero_step_towards(float *value, float target) {
+	float v_now = *value;
+	bool cross_zero = false;
+	if (target > 0) {
+		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET*1.5f) {
+			step_towards(value, target, 0.05f);
+			cross_zero = true;
+		}
+	}else if (target == 0) {
+		if (v_now >= 0 && v_now <= CONFIG_RAMP_SECOND_TARGET) {
+			step_towards(value, target, 0.05f);
+			cross_zero = true;
+		}
+	}else {
+		if (v_now >= -CONFIG_RAMP_SECOND_TARGET && v_now <= CONFIG_RAMP_SECOND_TARGET*1.5f) {
+			step_towards(value, target, 0.02f);
+			cross_zero = true;
+		}
+	}
+	if (!cross_zero) {
+		*value = target;
+	}
+}
+
+
 /* MPTA, 弱磁, 功率限制，主要是分配DQ轴电流 */
 static __INLINE void PMSM_FOC_FieldWeak(void) {
 	if (!gFoc_Ctrl.in.b_fwEnable) {
@@ -506,14 +531,15 @@ 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 = 0;
+			gFoc_Ctrl.pi_torque->min = -CONFIG_MAX_NEG_CURRENT;
 		}else {
 			gFoc_Ctrl.pi_torque->min = refTorque;
 			gFoc_Ctrl.pi_torque->max = 0;
 		}
 		float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
 		float maxTrq = PI_Controller_RunSat(gFoc_Ctrl.pi_torque, errRef);
-		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
+		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
+		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
 	}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
 		float maxSpeed = eCtrl_get_FinalSpeed();
 		float refSpeed = eCtrl_get_RefSpeed();
@@ -523,10 +549,10 @@ void PMSM_FOC_idqCalc(void) {
 		}
 		if (maxSpeed >= 0) {
 			gFoc_Ctrl.pi_speed->max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->min = -gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+			gFoc_Ctrl.pi_speed->min = -CONFIG_MAX_NEG_CURRENT;
 		}else if (maxSpeed < 0) {
 			gFoc_Ctrl.pi_speed->min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.phaseCurrLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed->max = gFoc_Ctrl.userLim.s_PhaseCurreBrkLim;
+			gFoc_Ctrl.pi_speed->max = CONFIG_MAX_NEG_CURRENT;
 		}
 
 		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
@@ -564,8 +590,8 @@ void PMSM_FOC_RunTime_Limit(void) {
 
 void PMSM_FOC_Slow_Task(void) {
 	eRamp_running(&gFoc_Ctrl.rtLim.phaseCurrLimRamp);
-	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
 	eRamp_running(&gFoc_Ctrl.rtLim.DCCurrLimRamp);
+	eRamp_running(&gFoc_Ctrl.rtLim.rpmLimRamp);
 	eRamp_running(&gFoc_Ctrl.in.cruiseRpmRamp);
 
 	PMSM_FOC_idqCalc();
@@ -622,10 +648,10 @@ float PMSM_FOC_GetDCCurrLimit(void) {
 }
 
 void PMSM_FOC_SpeedLimit(float speedLimit) {
-	PMSM_FOC_SpeedRampLimit(speedLimit, CONFIG_LIMIT_RAMP_TIME);
+	PMSM_FOC_SpeedRampLimit(speedLimit, CONFIG_LIMIT_RAMP_TIME, CONFIG_LIMIT_RAMP_TIME);
 }
 
-void PMSM_FOC_SpeedRampLimit(float speedLimit, u32 rampTime) {
+void PMSM_FOC_SpeedRampLimit(float speedLimit, u32 rampAccTime, u32 rampDecTime) {
 	if (speedLimit > gFoc_Ctrl.hwLim.s_motRPMMax) {
 		speedLimit = gFoc_Ctrl.hwLim.s_motRPMMax;
 	}
@@ -634,7 +660,7 @@ void PMSM_FOC_SpeedRampLimit(float speedLimit, u32 rampTime) {
 		need_reset = true;
 	}
 	gFoc_Ctrl.userLim.s_motRPMLim = (speedLimit);
-	eRamp_set_time(&gFoc_Ctrl.rtLim.rpmLimRamp, rampTime, rampTime);
+	eRamp_set_time(&gFoc_Ctrl.rtLim.rpmLimRamp, rampAccTime, rampDecTime);
 	if (need_reset) {
 		if ((gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) && gFoc_Ctrl.out.f_vdqRation >= 1.0f) {
 			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.out.s_RealCurrent);

Applications/foc/core/PMSM_FOC_Core.h

@@ -287,7 +287,7 @@ void PMSM_FOC_RT_LimInit(void);
 float PMSM_FOC_Get_Real_Torque(void);
 void PMSM_FOC_Reset_Torque(void);
 void PMSM_FOC_Torque_SpeedLimit(float speedLimit);
-void PMSM_FOC_SpeedRampLimit(float speedLimit, u32 rampTime);
+void PMSM_FOC_SpeedRampLimit(float speedLimit, u32 rampAccTime, u32 rampDecTime);
 
 #endif /* _PMSM_FOC_Core_H__ */
 

Applications/foc/foc_config.h

@@ -50,9 +50,11 @@
 
 #define CONFIG_RAMP_CROSS_ZERO_STEP (0.1F)
 
-#define CURRENT_LOOP_RAMP_COUNT 300
+#define CURRENT_LOOP_RAMP_COUNT 1
 
 #define CONFIG_TORQUE_MODE_MIN_RPM 600
 
+#define CONFIG_MAX_NEG_CURRENT 2.0F
+
 #endif /* _FOC_CONFIG_H__ */
 

Applications/foc/motor/motor.c

@@ -92,7 +92,7 @@ static void mc_gear_vmode_changed(void) {
 	}else {
 		gears = &nv_get_gear_configs()->gears_48[0];
 	}
-	PMSM_FOC_SpeedRampLimit(gears[motor.n_gear].u_maxRPM, 1000);
+	PMSM_FOC_SpeedRampLimit(gears[motor.n_gear].u_maxRPM, 500, 500);
 	PMSM_FOC_DCCurrLimit(gears[motor.n_gear].u_maxIdc);
 	PMSM_FOC_PhaseCurrLim(gears[motor.n_gear].u_maxTorque);
 }