锁电机功能部分实现

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

Applications/app/key_process.c

@@ -38,7 +38,8 @@ 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_SetCtrlMode(CTRL_MODE_SPD);
+			PMSM_FOC_LockMotor(true);
 		}
 		key_value[KEY_STOP] = value;
 	}

Applications/app/nv_storage.c

@@ -33,6 +33,7 @@ static void nv_default_motor_params(void) {
 	m_params.lq = MOTOR_Lq;
 	m_params.encoder_offset = -50;
 	m_params.est_pll_band = 200;
+	m_params.pos_lock_pll_band = 200;
 	m_params.flux_linkage = 0.0f;
 	m_params.hall_offset = 0;
 }

Applications/app/nv_storage.h

@@ -35,7 +35,8 @@ typedef struct {
 	s16   hall_offset;
 	s32   hall_table[8];
 	float encoder_offset;
-	float est_pll_band;
+	float est_pll_band; //normal工作模式下的pll带宽
+	float pos_lock_pll_band; //电机锁定模式下的pll带宽
 	u16   crc16;
 }motor_params_t;
 

Applications/foc/core/PMSM_FOC_Core.c

@@ -111,6 +111,7 @@ static void PMSM_FOC_Reset_PID(void) {
 	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_spd, 0);
 	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_fw, 0);
 	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_trq, 0);
+	PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
 }
 
 
@@ -120,7 +121,7 @@ void PMSM_FOC_CoreInit(void) {
 	_gFOC_Ctrl.pi_ctl_spd = &PI_Ctrl_Spd;
 	_gFOC_Ctrl.pi_ctl_fw = &PI_Ctrl_fw;
 	_gFOC_Ctrl.pi_ctl_trq = &PI_Ctrl_trq;
-	
+	_gFOC_Ctrl.pi_ctl_lock = &PI_Ctrl_lock;
 	memset(&_gFOC_Ctrl.in, 0, sizeof(_gFOC_Ctrl.in));
 	memset(&_gFOC_Ctrl.out, 0, sizeof(_gFOC_Ctrl.out));
 	_gFOC_Ctrl.params.s_maxiDC = nv_get_foc_params()->s_maxiDC;//(MAX_iDQ);
@@ -188,6 +189,22 @@ static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	}	
 }
 
+static __INLINE void PMSM_FOC_Plot_Debug(void) {
+	if (_gFOC_Ctrl.ctrl_count % 8 == 0) {
+		//plot_1data16(_gFOC_Ctrl.out.test_sample);
+		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
+		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
+		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
+		//plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
+		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
+		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
+		//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);
+	}
+}
+
 void PMSM_FOC_Schedule(void) {
 	AB_t vAB;
 #ifdef PHASE_LFP	
@@ -198,9 +215,6 @@ void PMSM_FOC_Schedule(void) {
 	_gFOC_Ctrl.ctrl_count++;
 
 	PMSM_FOC_Update_Hardware();
-	if (_gFOC_Ctrl.ctrl_count % 5 == 0) {
-		//plot_3data16(FtoS16x1000(iabc[0]), FtoS16x1000(iabc[1]), FtoS16x1000(iabc[2]));
-	}
 
 	if (_gFOC_Ctrl.out.n_RunMode != CTRL_MODE_OPEN) {
 
@@ -238,18 +252,7 @@ void PMSM_FOC_Schedule(void) {
 	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);
 #endif
-	if (_gFOC_Ctrl.ctrl_count % 8 == 0) {
-		//plot_1data16(_gFOC_Ctrl.out.test_sample);
-		//plot_1data16(FtoS16x1000(PMSM_FOC_Get_iDC()));
-		//plot_3data16(_gFOC_Ctrl.out.n_Duty[0], _gFOC_Ctrl.out.n_Duty[1], _gFOC_Ctrl.out.n_Duty[2]);
-		//plot_2data16(FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_RealIdq.q));
-		//plot_2data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.in.s_targetTorque));
-		//plot_2data16(FtoS16(_gFOC_Ctrl.in.s_hallAngle), FtoS16(_gFOC_Ctrl.in.s_motAngle));
-		//plot_3data16(_gFOC_Ctrl.in.s_motRPM, FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.d), FtoS16x1000(_gFOC_Ctrl.out.s_OutVdq.q));
-		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()));
-	}	
+	PMSM_FOC_Plot_Debug();
 }
 
 void PMSM_FOC_LogDebug(void) {
@@ -315,6 +318,13 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
+	if (_gFOC_Ctrl.in.b_motLock) {
+		float vel_count = motor_encoder_get_vel_count();
+		float errRef = 0 - vel_count;
+		_gFOC_Ctrl.in.s_targetTorque = PI_Controller_run(_gFOC_Ctrl.pi_ctl_lock ,errRef);
+		PMSM_FOC_idq_Assign();
+		return;
+	}
 	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) {
@@ -481,7 +491,11 @@ float PMSM_FOC_GetSpeed(void) {
 
 
 void PMSM_FOC_LockMotor(bool lock) {
-	_gFOC_Ctrl.in.b_motLock = lock;
+	if (_gFOC_Ctrl.in.b_motLock != lock) {		
+		motor_encoder_lock_pos(lock);
+		PI_Controller_Reset(_gFOC_Ctrl.pi_ctl_lock, 0);
+		_gFOC_Ctrl.in.b_motLock = lock;
+	}
 }
 
 void PMSM_FOC_SetSpdPid(float kp, float ki, float max, float min) {

Applications/foc/core/PMSM_FOC_Core.h

@@ -81,6 +81,7 @@ typedef struct {
 	PI_Controller *pi_ctl_spd;
 	PI_Controller *pi_ctl_fw;
 	PI_Controller *pi_ctl_trq;
+	PI_Controller *pi_ctl_lock;
 	dq_Rctrl      idq_ctl[2];
 	dq_Rctrl      vdq_ctl[2];	
 	FOC_InP       in;

Applications/foc/core/PMSM_FOC_Params.h

@@ -51,4 +51,12 @@ static PI_Controller PI_Ctrl_fw = {
 	.Ui = 0,
 };
 
+static PI_Controller PI_Ctrl_lock = {
+	.kp = (0.0001f),
+	.ki = (0.01f),
+	.max = (MAX_TORQUE),
+	.min = (-MAX_TORQUE),
+	.DT  = (1.0f/(float)SPD_CTRL_TS),
+	.Ui = 0,
+};
 

Applications/foc/motor/encoder.c

@@ -59,6 +59,17 @@ void encoder_init_clear(s8 diretcion) {
 	g_encoder.interpolation = 0.0f;
 }
 
+void encoder_lock_position(bool enable) {
+	if (g_encoder.b_lock_pos != enable) {
+		g_encoder.b_lock_pos = enable;
+		if (enable) {
+			encoder_set_bandwidth(nv_get_motor_params()->pos_lock_pll_band);
+		}else {
+			encoder_set_bandwidth(nv_get_motor_params()->est_pll_band);
+		}
+	}
+}
+
 static __INLINE void encoder_run_pll(float cnt) {
 	float pll_comp = 0.0f;
 	if (g_encoder.b_timer_ov) {
@@ -78,7 +89,6 @@ float encoder_get_theta(void) {
 	if (!g_encoder.b_index_found) {
 		return g_encoder.pwm_angle;
 	}
-	
 	u32 cnt = ENC_COUNT;
 	__NOP();__NOP();__NOP();__NOP();
 	if (ENC_OverFlow()) {
@@ -100,7 +110,6 @@ float encoder_get_theta(void) {
 	rand_angle(g_encoder.abi_angle);
 
 	encoder_run_pll((float)(cnt));
-
 	g_encoder.last_cnt = cnt;
 	g_encoder.last_us = timer_count32_get();
 	return g_encoder.abi_angle;
@@ -114,15 +123,23 @@ void encoder_detect_offset(float angle){
 	//plot_3data16(angle, g_encoder.pwm_angle, g_encoder.abi_angle);
 }
 
+float encoder_get_vel_count(void) {
+	return g_encoder.est_vel_counts;
+}
+
+static void encoder_sync_pwm_abs(void) {
+	ENC_COUNT = g_encoder.pwm_count;
+	g_encoder.last_cnt = g_encoder.pwm_count;
+	g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
+	g_encoder.abi_angle = g_encoder.pwm_angle;
+	g_encoder.b_index_found = true;
+}
 
 /*I 信号的中断处理，一圈一个中断*/
 void ENC_ABI_IRQHandler(void) {
 	g_encoder.b_index_cnt = ENC_COUNT;	
 	if (!g_encoder.b_index_found){
-		ENC_COUNT = g_encoder.pwm_count;
-		g_encoder.last_cnt = g_encoder.pwm_count;
-		g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
-		g_encoder.b_index_found = true;
+		encoder_sync_pwm_abs();
 	}
 }
 
@@ -146,10 +163,7 @@ void ENC_PWM_Duty_Handler(float t, float d) {
 	g_encoder.pwm_angle = ENC_Pluse_Nr_2_angle(Nr) * g_encoder.motor_poles + g_encoder.enc_offset;	
 	rand_angle(g_encoder.pwm_angle);
 	if (!g_encoder.b_index_found) {
-		ENC_COUNT = g_encoder.pwm_count;
-		g_encoder.last_cnt = g_encoder.pwm_count;
-		g_encoder.est_pll.observer = (float)g_encoder.pwm_count;
-		g_encoder.b_index_found = true;
+		encoder_sync_pwm_abs();
 	}
 }
 

Applications/foc/motor/encoder.h

@@ -12,8 +12,10 @@ typedef struct {
 	float interpolation;
 	float pll_bandwidth_shadow;
 	float pll_bandwidth;
+	float postion;
 	PLL_t est_pll;
 	bool  b_timer_ov;
+	bool  b_lock_pos;
 	u32   cpr;
 	u32   last_cnt;
 	u32   b_index_cnt;
@@ -33,6 +35,8 @@ float encoder_get_theta(void);
 float encoder_get_speed(void);
 void encoder_detect_offset(float angle);
 void encoder_set_direction(s8 direction);
+float encoder_get_vel_count(void);
+void encoder_lock_position(bool enable);
 
 #endif /* _Encoder_H__ */
 

Applications/foc/motor/motor.h

@@ -43,6 +43,15 @@ static __INLINE float motor_encoder_get_speed(void) {
 #endif
 }
 
+static __INLINE float motor_encoder_get_vel_count(void) {
+#ifdef USE_ENCODER_HALL
+	return 0;
+#elif defined (USE_ENCODER_ABI)
+	return encoder_get_vel_count();
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
 
 static __INLINE void motor_encoder_init(void) {
 #ifdef USE_ENCODER_HALL
@@ -93,5 +102,16 @@ static __INLINE void motor_encoder_set_direction(s8 dir) {
 #endif
 }
 
+static __INLINE void motor_encoder_lock_pos(bool lock) {
+#ifdef USE_ENCODER_HALL
+		
+#elif defined (USE_ENCODER_ABI)
+		encoder_lock_position(lock);
+#else
+	#error "Postion sensor ERROR"
+#endif
+}
+
+
 #endif /* _MOTOR_H__ */
 

Applications/os/os_types.h

@@ -46,6 +46,7 @@ typedef unsigned int	uint32;
 #define PHASE_360_DEGREE (360.0f)
 
 #define rand_angle(a) {while (a >= PHASE_360_DEGREE) a-=PHASE_360_DEGREE;while (a < 0) a +=PHASE_360_DEGREE;};
+#define ABS(x) 					( (x)>0?(x):-(x) )
 
 extern void *pvPortMalloc( size_t xWantedSize );
 #define os_alloc pvPortMalloc

Project/GD32_DEMO.uvoptx

@@ -120,7 +120,6 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name>?</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>