foc的pid控制器单独定义

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

Applications/foc/core/PMSM_FOC_Core.c

@@ -1,6 +1,7 @@
 #include "arm_math.h"
 #include "PMSM_FOC_Core.h"
-#include "PMSM_FOC_Params.h"
+#include "foc/foc_config.h"
+#include "foc/motor/motor_param.h"
 #include "foc/core/e_ctrl.h"
 #include "math/fix_math.h"
 #include "math/fast_math.h"
@@ -19,7 +20,6 @@
 #define _DEBUG(fmt, args...) sys_debug(fmt, ##args)
 
 PMSM_FOC_Ctrl gFoc_Ctrl;
-static Fir_t phase1, phase2;
 static bool g_focinit = false;
 
 static u32 PMSM_FOC_Debug_Task(void *p);
@@ -116,58 +116,58 @@ static __INLINE void FOC_Set_vDqRamp(dq_Rctrl *c, float target) {
 
 
 static void PMSM_FOC_Reset_PID(void) {
-	PI_Controller_Reset(gFoc_Ctrl.pi_id, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_iq, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_speed, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_fw, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_torque, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
-	PI_Controller_Reset(gFoc_Ctrl.pi_power, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_id, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_iq, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_fw, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_power, 0);
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, 0);
 	ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, 0);
+#else
+	PI_Controller_Reset(&gFoc_Ctrl.pi_speed, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_torque, 0);
 #endif
 }
 
 static void PMSM_FOC_Conf_PID(void) {
-	gFoc_Ctrl.pi_id->kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
-	gFoc_Ctrl.pi_id->ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
-	gFoc_Ctrl.pi_id->kd = nv_get_foc_params()->pid_conf[PID_D_id].kd;
-	gFoc_Ctrl.pi_id->DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
-
-	gFoc_Ctrl.pi_iq->kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
-	gFoc_Ctrl.pi_iq->ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
-	gFoc_Ctrl.pi_iq->kd = nv_get_foc_params()->pid_conf[PID_Q_id].kd;
-	gFoc_Ctrl.pi_iq->DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
-
-	gFoc_Ctrl.pi_torque->kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
-	gFoc_Ctrl.pi_torque->ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
-	gFoc_Ctrl.pi_torque->kd = nv_get_foc_params()->pid_conf[PID_TRQ_id].kd;
-	gFoc_Ctrl.pi_torque->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-
-	gFoc_Ctrl.pi_speed->kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
-	gFoc_Ctrl.pi_speed->ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
-	gFoc_Ctrl.pi_speed->kd = nv_get_foc_params()->pid_conf[PID_Spd_id].kd;
-	gFoc_Ctrl.pi_speed->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-
-	gFoc_Ctrl.pi_power->kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
-	gFoc_Ctrl.pi_power->ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
-	gFoc_Ctrl.pi_power->kd = nv_get_foc_params()->pid_conf[PID_Pow_id].kd;
-	gFoc_Ctrl.pi_power->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-
-	gFoc_Ctrl.pi_lock->kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
-	gFoc_Ctrl.pi_lock->ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
-	gFoc_Ctrl.pi_lock->kd = nv_get_foc_params()->pid_conf[PID_Lock_id].kd;
-	gFoc_Ctrl.pi_lock->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-
-	gFoc_Ctrl.pi_fw->kp = nv_get_foc_params()->pid_conf[PID_FW_id].kp;
-	gFoc_Ctrl.pi_fw->ki = nv_get_foc_params()->pid_conf[PID_FW_id].ki;
-	gFoc_Ctrl.pi_fw->kd = nv_get_foc_params()->pid_conf[PID_FW_id].kd;
-	gFoc_Ctrl.pi_fw->DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
-	PI_Controller_max(gFoc_Ctrl.pi_fw, 0, -CONFIG_MAX_FW_D_CURR);
+	gFoc_Ctrl.pi_id.kp = nv_get_foc_params()->pid_conf[PID_D_id].kp;
+	gFoc_Ctrl.pi_id.ki = nv_get_foc_params()->pid_conf[PID_D_id].ki;
+	gFoc_Ctrl.pi_id.kd = nv_get_foc_params()->pid_conf[PID_D_id].kd;
+	gFoc_Ctrl.pi_id.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
+
+	gFoc_Ctrl.pi_iq.kp = nv_get_foc_params()->pid_conf[PID_Q_id].kp;
+	gFoc_Ctrl.pi_iq.ki = nv_get_foc_params()->pid_conf[PID_Q_id].ki;
+	gFoc_Ctrl.pi_iq.kd = nv_get_foc_params()->pid_conf[PID_Q_id].kd;
+	gFoc_Ctrl.pi_iq.DT = (1.0f/(float)CONFIG_IDQ_CTRL_TS);
+
+	gFoc_Ctrl.pi_power.kp = nv_get_foc_params()->pid_conf[PID_Pow_id].kp;
+	gFoc_Ctrl.pi_power.ki = nv_get_foc_params()->pid_conf[PID_Pow_id].ki;
+	gFoc_Ctrl.pi_power.kd = nv_get_foc_params()->pid_conf[PID_Pow_id].kd;
+	gFoc_Ctrl.pi_power.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+
+	gFoc_Ctrl.pi_lock.kp = nv_get_foc_params()->pid_conf[PID_Lock_id].kp;
+	gFoc_Ctrl.pi_lock.ki = nv_get_foc_params()->pid_conf[PID_Lock_id].ki;
+	gFoc_Ctrl.pi_lock.kd = nv_get_foc_params()->pid_conf[PID_Lock_id].kd;
+	gFoc_Ctrl.pi_lock.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+
+	gFoc_Ctrl.pi_fw.kp = nv_get_foc_params()->pid_conf[PID_FW_id].kp;
+	gFoc_Ctrl.pi_fw.ki = nv_get_foc_params()->pid_conf[PID_FW_id].ki;
+	gFoc_Ctrl.pi_fw.kd = nv_get_foc_params()->pid_conf[PID_FW_id].kd;
+	gFoc_Ctrl.pi_fw.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	PI_Controller_max(&gFoc_Ctrl.pi_fw, 0, -CONFIG_MAX_FW_D_CURR);
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
 	ladrc_init(&gFoc_Ctrl.vel_adrc, 1.0f/(float)CONFIG_SPD_CTRL_TS, CONFIG_LADRC_Wo, CONFIG_LADRC_Wcv, CONFIG_LADRC_B0);
+#else
+	gFoc_Ctrl.pi_torque.kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
+	gFoc_Ctrl.pi_torque.ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
+	gFoc_Ctrl.pi_torque.kd = nv_get_foc_params()->pid_conf[PID_TRQ_id].kd;
+	gFoc_Ctrl.pi_torque.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
+	gFoc_Ctrl.pi_speed.kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
+	gFoc_Ctrl.pi_speed.ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
+	gFoc_Ctrl.pi_speed.kd = nv_get_foc_params()->pid_conf[PID_Spd_id].kd;
+	gFoc_Ctrl.pi_speed.DT = (1.0f/(float)CONFIG_SPD_CTRL_TS);
 #endif
 }
 
@@ -194,8 +194,7 @@ void PMSM_FOC_RT_LimInit(void) {
 }
 
 void PMSM_FOC_CoreInit(void) {
-	Fir_init(&phase1);
-	Fir_init(&phase2);
+#if 0
 	gFoc_Ctrl.pi_id = &PI_Ctrl_ID;
 	gFoc_Ctrl.pi_iq = &PI_Ctrl_IQ;
 	gFoc_Ctrl.pi_speed = &PI_Ctrl_Spd;
@@ -203,7 +202,7 @@ void PMSM_FOC_CoreInit(void) {
 	gFoc_Ctrl.pi_torque = &PI_Ctrl_trq;
 	gFoc_Ctrl.pi_lock = &PI_Ctrl_lock;
 	gFoc_Ctrl.pi_power = &PI_Ctrl_Power;
-
+#endif
 	PMSM_FOC_Conf_PID();
 	
 	memset(&gFoc_Ctrl.in, 0, sizeof(gFoc_Ctrl.in));
@@ -322,18 +321,18 @@ static __INLINE void PMSM_FOC_Update_PI_Idq(void) {
 	gFoc_Ctrl.params.maxvDQ.q = gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
 	gFoc_Ctrl.params.minvDQ.q = -gFoc_Ctrl.params.f_DCLim * gFoc_Ctrl.params.n_modulation;//CONFIG_RATED_DC_VOL;
 
-	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id->max) {
-		gFoc_Ctrl.pi_id->max = gFoc_Ctrl.params.maxvDQ.d;
+	if (gFoc_Ctrl.params.maxvDQ.d != gFoc_Ctrl.pi_id.max) {
+		gFoc_Ctrl.pi_id.max = gFoc_Ctrl.params.maxvDQ.d;
 	}
-	if (gFoc_Ctrl.params.minvDQ.d != gFoc_Ctrl.pi_id->min) {
-		gFoc_Ctrl.pi_id->min = gFoc_Ctrl.params.minvDQ.d;
+	if (gFoc_Ctrl.params.minvDQ.d != gFoc_Ctrl.pi_id.min) {
+		gFoc_Ctrl.pi_id.min = gFoc_Ctrl.params.minvDQ.d;
 	}
 	/* update iq pi ctrl */
-	if (gFoc_Ctrl.params.maxvDQ.q != gFoc_Ctrl.pi_iq->max) {
-		gFoc_Ctrl.pi_iq->max = gFoc_Ctrl.params.maxvDQ.q;
+	if (gFoc_Ctrl.params.maxvDQ.q != gFoc_Ctrl.pi_iq.max) {
+		gFoc_Ctrl.pi_iq.max = gFoc_Ctrl.params.maxvDQ.q;
 	}
-	if (gFoc_Ctrl.params.minvDQ.q != gFoc_Ctrl.pi_iq->min) {
-		gFoc_Ctrl.pi_iq->min = gFoc_Ctrl.params.minvDQ.q;
+	if (gFoc_Ctrl.params.minvDQ.q != gFoc_Ctrl.pi_iq.min) {
+		gFoc_Ctrl.pi_iq.min = gFoc_Ctrl.params.minvDQ.q;
 	}	
 }
 
@@ -379,12 +378,12 @@ void PMSM_FOC_Schedule(void) {
 		float target_d = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[0]);
 	#endif
 		float err = target_d - gFoc_Ctrl.out.s_RealIdq.d;
-		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(gFoc_Ctrl.pi_id, err);
+		gFoc_Ctrl.in.s_targetVdq.d = PI_Controller_RunSerial(&gFoc_Ctrl.pi_id, err);
 	#ifndef CONFIG_DQ_STEP_RESPONSE
 		float target_q = FOC_Get_DqRamp(&gFoc_Ctrl.idq_ctl[1]);
 	#endif
 		err = target_q - gFoc_Ctrl.out.s_RealIdq.q;
-		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(gFoc_Ctrl.pi_iq, err);
+		gFoc_Ctrl.in.s_targetVdq.q = PI_Controller_RunSerial(&gFoc_Ctrl.pi_iq, err);
 
 #if 0
 		float eW = gFoc_Ctrl.in.s_motRPM * PI/ 30.0f * gFoc_Ctrl.params.n_poles; //电角速度
@@ -466,7 +465,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 			//ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
 			ladrc_copy(&gFoc_Ctrl.vel_lim_adrc, &gFoc_Ctrl.vel_adrc);
 #else
-			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
 #endif
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 #ifdef CONFIG_SPEED_LADRC
@@ -477,10 +476,14 @@ u8 PMSM_FOC_CtrlMode(void) {
 			if (gFoc_Ctrl.pi_id->is_sat || gFoc_Ctrl.pi_iq->is_sat) {
 				target_troque = PMSM_FOC_Get_Real_Torque();
 			}
-			PI_Controller_Reset(gFoc_Ctrl.pi_speed, target_troque);
+			PI_Controller_Reset(&gFoc_Ctrl.pi_speed, target_troque);
 #endif
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
-			PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+#ifdef CONFIG_SPEED_LADRC
+			ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motRPM, gFoc_Ctrl.in.s_targetTorque);
+#else
+			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+#endif
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
 			float real_trq = PMSM_FOC_Get_Real_Torque() * 0.9f;
 			eCtrl_reset_Current(min(real_trq, gFoc_Ctrl.in.s_targetTorque));
@@ -524,9 +527,9 @@ static __INLINE void PMSM_FOC_FieldWeak(void) {
 }
 static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
 #if 1
-	gFoc_Ctrl.pi_power->max = maxTrq;
+	gFoc_Ctrl.pi_power.max = maxTrq;
 	float errRef = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.DCCurrLimRamp) - (gFoc_Ctrl.out.s_FilteriDC);
-	return PI_Controller_Run(gFoc_Ctrl.pi_power, errRef);
+	return PI_Controller_Run(&gFoc_Ctrl.pi_power, errRef);
 #else
 	return maxTrq;
 #endif
@@ -543,7 +546,7 @@ static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 	gFoc_Ctrl.pi_torque->min = 0;
 
 	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motRPM;
-	return PI_Controller_RunLimit(gFoc_Ctrl.pi_torque, err);
+	return PI_Controller_RunLimit(&gFoc_Ctrl.pi_torque, err);
 #else
 	return maxTrq;
 #endif
@@ -580,11 +583,11 @@ static __INLINE void PMSM_FOC_idq_Assign(void) {
 /*called in media task */
 void PMSM_FOC_idqCalc(void) {
 	if (gFoc_Ctrl.in.b_AutoHold) {
-		gFoc_Ctrl.pi_lock->max = CONFIG_DEFAULT_LOCK_TORQUE_LIM;
-		gFoc_Ctrl.pi_lock->min = -CONFIG_DEFAULT_LOCK_TORQUE_LIM;
+		gFoc_Ctrl.pi_lock.max = CONFIG_DEFAULT_LOCK_TORQUE_LIM;
+		gFoc_Ctrl.pi_lock.min = -CONFIG_DEFAULT_LOCK_TORQUE_LIM;
 		float vel_count = motor_encoder_get_vel_count();
 		float errRef = 0 - vel_count;
-		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(gFoc_Ctrl.pi_lock ,errRef);
+		gFoc_Ctrl.in.s_targetTorque = PI_Controller_Run(&gFoc_Ctrl.pi_lock ,errRef);
 		PMSM_FOC_idq_Assign();
 		return;
 	}
@@ -634,7 +637,7 @@ void PMSM_FOC_idqCalc(void) {
 		}
 		gFoc_Ctrl.in.s_targetRPM = refSpeed;
 		float errRef = refSpeed - gFoc_Ctrl.in.s_motRPM;
-		float maxTrq = PI_Controller_Run(gFoc_Ctrl.pi_speed, errRef);
+		float maxTrq = PI_Controller_Run(&gFoc_Ctrl.pi_speed, errRef);
 #endif
 		gFoc_Ctrl.in.s_targetTorque = PMSM_FOC_Limit_iDC(maxTrq);
 	}
@@ -970,13 +973,21 @@ float PMSM_FOC_GetSpeed(void) {
 void PMSM_FOC_AutoHold(bool lock) {
 	if (gFoc_Ctrl.in.b_AutoHold != lock) {
 		motor_encoder_lock_pos(lock);
-		PI_Controller_Reset(gFoc_Ctrl.pi_lock, 0);
+		PI_Controller_Reset(&gFoc_Ctrl.pi_lock, 0);
 		if (!lock) {
 			//解锁后为了防止倒溜，需要把当前
 			if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ) {
-				PI_Controller_Reset(gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+#ifdef CONFIG_SPEED_LADRC
+				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, gFoc_Ctrl.in.s_targetTorque);
+#else
+				PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+#endif
 			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
-				PI_Controller_Reset(gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+#ifdef CONFIG_SPEED_LADRC
+				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, gFoc_Ctrl.in.s_targetTorque);
+#else
+				PI_Controller_Reset(&gFoc_Ctrl.pi_speed, gFoc_Ctrl.in.s_targetTorque);
+#endif
 			}
 			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
 		}
@@ -992,13 +1003,17 @@ bool PMSM_FOC_AutoHoldding(void) {
 static PI_Controller *_pid(u8 id) {
 	PI_Controller *pi = NULL;
 	if (id == PID_D_id) {
-		pi = gFoc_Ctrl.pi_id;
+		pi = &gFoc_Ctrl.pi_id;
 	}else if (id == PID_Q_id) {
-		pi = gFoc_Ctrl.pi_iq;
+		pi = &gFoc_Ctrl.pi_iq;
 	}else if (id == PID_TRQ_id) {
-		pi = gFoc_Ctrl.pi_torque;
+#ifndef CONFIG_SPEED_LADRC
+		pi = &gFoc_Ctrl.pi_torque;
+#endif
 	}else if (id == PID_Spd_id) {
-		pi = gFoc_Ctrl.pi_speed;
+#ifndef CONFIG_SPEED_LADRC
+		pi = &gFoc_Ctrl.pi_speed;
+#endif
 	}
 	return pi;
 }

Applications/foc/core/PMSM_FOC_Core.h

@@ -180,16 +180,17 @@ typedef struct {
 }dq_Rctrl; //dq ramp ctrl
 
 typedef struct {
-	PI_Controller *pi_id;
-	PI_Controller *pi_iq;
-	PI_Controller *pi_speed;
-	PI_Controller *pi_fw;
-	PI_Controller *pi_torque;
-	PI_Controller *pi_lock;
-	PI_Controller *pi_power;
+	PI_Controller pi_id;
+	PI_Controller pi_iq;	
+	PI_Controller pi_fw;
+	PI_Controller pi_lock;
+	PI_Controller pi_power;
 #ifdef CONFIG_SPEED_LADRC
 	ladrc_t        vel_lim_adrc;
 	ladrc_t        vel_adrc;
+#else
+	PI_Controller pi_torque;
+	PI_Controller pi_speed;
 #endif
 	dq_Rctrl      idq_ctl[2];
 	dq_Rctrl      vdq_ctl[2];	

Applications/foc/core/PMSM_FOC_Params.h

@@ -1,75 +0,0 @@
-#include "foc/foc_config.h"
-#include "foc/core/PI_Controller.h"
-#include "foc/motor/motor_param.h"
-
-static PI_Controller PI_Ctrl_ID = {
-	//.kp = (CURRENT_BANDWITH * MOTOR_Ld),
-	//.ki = (CURRENT_BANDWITH * MOTOR_R),
-	//.kp = (CURRENT_BANDWITH * MOTOR_Ld),
-	//.ki = (MOTOR_R/MOTOR_Ld),
-	.max = (CONFIG_RATED_DC_VOL),
-	.min = (-CONFIG_RATED_DC_VOL),
-	.DT  = (1.0f/(float)CONFIG_IDQ_CTRL_TS),
-	.Ui = 0,
-};
-
-static PI_Controller PI_Ctrl_IQ = {
-	//.kp = (CURRENT_BANDWITH * MOTOR_Lq),
-	//.ki = (CURRENT_BANDWITH * MOTOR_R),
-	//.kp = (CURRENT_BANDWITH * MOTOR_Lq),
-	//.ki = (MOTOR_R/MOTOR_Lq),
-	.max = (CONFIG_RATED_DC_VOL),
-	.min = (-CONFIG_RATED_DC_VOL),
-	.DT  = (1.0f/(float)CONFIG_IDQ_CTRL_TS),
-	.Ui = 0,
-};
-
-static PI_Controller PI_Ctrl_trq = {
-	//.kp = 0.1f,
-	//.ki = 0.5f,
-	//.kd = 1.2f,
-	.max = (0),
-	.min = (0),
-	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
-	.Ui = 0,
-};
-
-static PI_Controller PI_Ctrl_Spd = {
-	//.kp = 0.1f,
-	//.ki = 0.5f,
-	//.kd = 1.2f,
-	.max = (0),
-	.min = (-0),
-	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
-	.Ui = 0,
-};
-
-
-static PI_Controller PI_Ctrl_Power = {
-	//.kp = (0.5f),
-	//.ki = (100.0f),
-	.max = (0),
-	.min = 0,
-	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
-	.Ui = 0,
-};
-
-static PI_Controller PI_Ctrl_lock = {
-	//.kp = (0.0001f),
-	//.ki = (0.01f),
-	.max = (0),
-	.min = (-0),
-	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
-	.Ui = 0,
-};
-
-static PI_Controller PI_Ctrl_fw = {
-	//.kp = (0.001f),
-	//.ki = (0.003f),
-	.max = (20),
-	.min = (0),
-	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
-	.Ui = 0,
-};
-
-