速度，扭矩限速PI调试

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

Applications/app/app.c

@@ -88,9 +88,13 @@ void app_start(void){
 }
 
 static u32 _app_report_task(void *p) {
+	static u32 loop = 0;
 	can_report_power(0x45, (s16)PMSM_FOC_GetSpeed(), PMSM_FOC_GetVbusVoltage(), PMSM_FOC_GetVbusCurrent());
 	can_report_dq_current(0x45, PMSM_FOC_GetDQCurrent()->d, PMSM_FOC_GetDQCurrent()->q);
-	can_report_pid_value(0x45, PID_Q_id);
+	if (++loop % 10 == 0) {
+		can_report_pid_value(0x45, PID_TRQ_id);
+		sys_debug("start time %d\n", shark_get_seconds());
+	}
 	return 500;
 }
 

Applications/app/nv_storage.c

@@ -63,12 +63,12 @@ static void nv_default_foc_params(void) {
 	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.1f;
-	foc_params.pid_conf[PID_TRQ_id].ki = 0.5f;
+	foc_params.pid_conf[PID_TRQ_id].kp = 0.13f;
+	foc_params.pid_conf[PID_TRQ_id].ki = 0.4f;
 	foc_params.pid_conf[PID_TRQ_id].kb = 1.0f;
 
-	foc_params.pid_conf[PID_Spd_id].kp = 0.05f;
-	foc_params.pid_conf[PID_Spd_id].ki = 0.15f;
+	foc_params.pid_conf[PID_Spd_id].kp = 0.13f;
+	foc_params.pid_conf[PID_Spd_id].ki = 0.08f;
 	foc_params.pid_conf[PID_Spd_id].kb = 1.2f;
 
 	foc_params.pid_conf[PID_Pow_id].kp = 0.5f;
@@ -129,6 +129,7 @@ void nv_read_foc_params(void) {
 
 void nv_set_pid(u8 id, pid_conf_t *pid) {
 	foc_params.pid_conf[id] = *pid;
+	nv_save_foc_params();
 }
 
 void nv_get_pid(u8 id, pid_conf_t *pid) {
@@ -153,6 +154,6 @@ void nv_storage_init(void) {
 	nv_read_motor_params();
 	nv_read_foc_params();
 	nv_default_motor_params();
-	nv_default_foc_params();
+	//nv_default_foc_params();
 }
 

Applications/foc/commands.c

@@ -42,12 +42,6 @@ static u32 foc_command_task(void *args) {
 	return 0;
 }
 
-#if 0
-static void conf_foc_pid(u8 id, pid_conf_t *pid) {
-	nv_set_pid(id, pid);
-}
-#endif
-
 static void process_foc_command(foc_cmd_body_t *command) {
 	u8 erroCode = 0;
 	u8 response[32];
@@ -168,7 +162,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
 			sys_debug("id = %d, kp = %f, ki = %f, kb = %f\n", id, pid.kp, pid.ki, pid.kb);
 			PMSM_FOC_SetPid(id, pid.kp, pid.ki, pid.kb);
-			//conf_foc_pid(id, &pid);
+			nv_set_pid(id, &pid);
 			break;
 		}
 		case Foc_Set_EPM_Mode:

Applications/foc/core/PMSM_FOC_Core.c

@@ -245,7 +245,7 @@ static __INLINE void PMSM_FOC_Update_Hardware(void) {
 	SinCos_Lut(_gFOC_Ctrl.in.s_motAngle, &_gFOC_Ctrl.out.sin, &_gFOC_Ctrl.out.cos);
 	
 	_gFOC_Ctrl.in.s_motRPM = motor_encoder_get_speed();
-	_gFOC_Ctrl.in.s_vDC = get_vbus_float();
+	_gFOC_Ctrl.in.s_vDC = get_vbus_int();
 	//sample current
 	phase_current_get(_gFOC_Ctrl.in.s_iABC);
 	get_phase_vols(_gFOC_Ctrl.in.s_vABC);
@@ -297,12 +297,12 @@ static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 
 static u32 PMSM_FOC_Debug_Task(void *p) {
 	if (_gFOC_Ctrl.in.b_motEnable) {
-		plot_3data16(FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[0]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[1]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[2]));
-		//plot_3data16(FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[0]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[1]), _gFOC_Ctrl.in.s_motRPM);
+		//plot_3data16(FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[0]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[1]), FtoS16x10(_gFOC_Ctrl.in.s_iABCFilter[2]));
+		plot_3data16(_gFOC_Ctrl.in.s_targetTorque, _gFOC_Ctrl.in.s_targetRPM, _gFOC_Ctrl.in.s_motRPM);
 		//plot_3data16(FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x10(_gFOC_Ctrl.out.s_RealIdq.q), FtoS16x10(_gFOC_Ctrl.idq_ctl[1].s_FinalTgt));
 		//plot_3data16( _gFOC_Ctrl.in.s_motRPM, speed_td.target, speed_td.diff);
 	}
-	return 0;
+	return 1;
 }
 
 void PMSM_FOC_Schedule(void) {
@@ -431,7 +431,7 @@ void PMSM_FOC_idqCalc(void) {
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
 		float refTorque = eCtrl_get_FinalTorque();
 		_gFOC_Ctrl.pi_ctl_trq->max = refTorque;
-		_gFOC_Ctrl.pi_ctl_trq->min = -refTorque;
+		_gFOC_Ctrl.pi_ctl_trq->min = 0;
 		if ((eCtrl_get_FinalTorque() <= 0.0001f) && (_gFOC_Ctrl.in.s_motRPM < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
 			_gFOC_Ctrl.pi_ctl_trq->max = 0;
 			_gFOC_Ctrl.pi_ctl_trq->min = 0; //防止倒转
@@ -441,7 +441,7 @@ void PMSM_FOC_idqCalc(void) {
 		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
 	}else if (_gFOC_Ctrl.out.n_RunMode == CTRL_MODE_SPD){
 		_gFOC_Ctrl.pi_ctl_spd->max = _gFOC_Ctrl.userLim.s_PhaseCurrLim;
-		_gFOC_Ctrl.pi_ctl_spd->min = -_gFOC_Ctrl.userLim.s_PhaseCurrLim;
+		_gFOC_Ctrl.pi_ctl_spd->min = 0;//-_gFOC_Ctrl.userLim.s_PhaseCurrLim;
 		float refSpeed = eCtrl_get_RefSpeed();
 		if (_gFOC_Ctrl.in.b_cruiseEna) {
 			refSpeed = _gFOC_Ctrl.in.s_cruiseRPM;
@@ -451,6 +451,7 @@ void PMSM_FOC_idqCalc(void) {
 				_gFOC_Ctrl.pi_ctl_spd->min = 0; //防止倒转
 			}
 		}
+		_gFOC_Ctrl.in.s_targetRPM = refSpeed;
 		float errRef = refSpeed - _gFOC_Ctrl.in.s_motRPM;
 		float maxTrq = PI_Controller_RunSat(_gFOC_Ctrl.pi_ctl_spd, errRef);
 		_gFOC_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_Power(maxTrq);
@@ -693,16 +694,25 @@ void PMSM_FOC_LockMotor(bool lock) {
 	}
 }
 
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {
-	if (id > PID_Max_id) {
-		return;
-	}
+static PI_Controller *_pid(u8 id) {
 	PI_Controller *pi = NULL;
 	if (id == PID_D_id) {
 		pi = _gFOC_Ctrl.pi_ctl_id;
 	}else if (id == PID_Q_id) {
 		pi = _gFOC_Ctrl.pi_ctl_iq;
+	}else if (id == PID_TRQ_id) {
+		pi = _gFOC_Ctrl.pi_ctl_trq;
+	}else if (id == PID_Spd_id) {
+		pi = _gFOC_Ctrl.pi_ctl_spd;
 	}
+	return pi;
+}
+
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {
+	if (id > PID_Max_id) {
+		return;
+	}
+	PI_Controller *pi = _pid(id);
 	if (pi != NULL) {
 		pi->kp = kp;
 		pi->ki = ki;
@@ -714,12 +724,7 @@ void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb) {
 	if (id > PID_Max_id) {
 		return;
 	}
-	PI_Controller *pi = NULL;
-	if (id == PID_D_id) {
-		pi = _gFOC_Ctrl.pi_ctl_id;
-	}else if (id == PID_Q_id) {
-		pi = _gFOC_Ctrl.pi_ctl_iq;
-	}
+	PI_Controller *pi = _pid(id);
 	if (pi != NULL) {
 		*kp = pi->kp;
 		*ki = pi->ki;
@@ -763,7 +768,7 @@ float PMSM_FOC_Calc_iDC(void) {
 		iDC x vDC = 2/3(iq x vq + id x vd);
 	*/
 	float m_pow = (vd * id + vq * iq); //s32q10
-	float raw_idc = m_pow / _gFOC_Ctrl.in.s_vDC * 1.5f; //s16q5
+	float raw_idc = m_pow / _gFOC_Ctrl.in.s_vDC;// * 1.5f * 0.66f; //s16q5
 	LowPass_Filter(_gFOC_Ctrl.out.s_FilteriDC, raw_idc, 0.01f);
 	return _gFOC_Ctrl.out.s_FilteriDC;
 }

Applications/foc/motor/motor.c

@@ -142,8 +142,10 @@ bool mc_set_foc_mode(u8 mode) {
 	bool ret = false;
 	if (PMSM_FOC_SetCtrlMode(mode)) {
 		motor.mode = mode;
-		PMSM_FOC_Start(motor.mode);
-		pwm_enable_channel();
+		if (mode == CTRL_MODE_OPEN) {
+			PMSM_FOC_Start(motor.mode);
+			pwm_enable_channel();
+		}
 		ret = true;
 	}
 	cpu_exit_critical(mask);