1. 默认关闭log，有45发来的can指令，打开log

2. 限速和速度环，使用PI 控制器控制

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

Applications/app/app.c

@@ -100,7 +100,7 @@ static u32 _app_trq_test_task(void *args) {
 }
 #endif
 void app_start(void){
-	set_log_level(MOD_SYSTEM, L_debug);
+	set_log_level(MOD_SYSTEM, L_disable);
 	can_message_init();
 	nv_storage_init();
 	mc_init();
@@ -118,13 +118,18 @@ void app_start(void){
 
 static u32 _app_report_task(void *p) {
 	static u32 loop = 0;
+
+	can_report_ext_status(0x43);
+	if (!can_is_connect_pc()) {
+		return 200;
+	}
 	can_report_power(0x45);
 	can_report_dq_current(0x45);
 	can_report_foc_status(0x45);
 	can_report_phase_voltage(0x45);
 	can_report_mpta_values(0x45);
-	can_report_ext_status(0x43);
 	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);
@@ -164,8 +169,8 @@ static void plot_smo_angle(void) {
 }
 static u32 _app_plot_task(void * args) {
 	if (plot_type == 1) {
-		can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
-		//can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
+		//can_plot2((s16)foc_observer_sensorless_speed(), (s16)PMSM_FOC_GetSpeed());
+		can_plot3((s16)PMSM_FOC_GetSpeed() + 1000, (s16)PMSM_FOC_GetSpeed(), (s16)PMSM_FOC_GetSpeed() - 1000);
 	}else if (plot_type == 2) {
 		can_plot2(eCtrl_get_FinalTorque(), PMSM_FOC_Get()->in.s_targetTorque);
 	}else if (plot_type == 3) {

Applications/app/nv_storage.c

@@ -103,6 +103,7 @@ static void nv_default_limter(void) {
 	limiter.vbus.limit_value = 20;
 }
 
+
 static void nv_default_foc_params(void) {
 	foc_params.s_maxDCVol = CONFIG_MAX_DC_VOL;
 	foc_params.s_minDCVol = CONFIG_MIN_DC_VOL;
@@ -136,13 +137,13 @@ 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].kd = 0;
 
-	foc_params.pid_conf[PID_TRQ_id].kp = TRQ_PI_KP;
-	foc_params.pid_conf[PID_TRQ_id].ki = TRQ_PI_KI;
-	foc_params.pid_conf[PID_TRQ_id].kd = TRO_PI_KD;
+	foc_params.pid_conf[PID_TRQ_id].kp = PI_VEL_LIM_KP;
+	foc_params.pid_conf[PID_TRQ_id].ki = PI_VEL_LIM_KI;
+	foc_params.pid_conf[PID_TRQ_id].kd = PI_VEL_LIM_KD;
 
-	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].kd = 0.0f;
+	foc_params.pid_conf[PID_Spd_id].kp = PI_VEL_KP;
+	foc_params.pid_conf[PID_Spd_id].ki = PI_VEL_KI;
+	foc_params.pid_conf[PID_Spd_id].kd = PI_VEL_KD;
 
 	foc_params.pid_conf[PID_Pow_id].kp = 5.0f;
 	foc_params.pid_conf[PID_Pow_id].ki = 15.0f;
@@ -605,12 +606,13 @@ void nv_storage_init(void) {
 #endif
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_ZD_100
 	m_params.offset = 0.0f; //编码器做了零位置校准
+	m_params.est_pll_band = 200;
 #endif
 #if CONFIG_MOT_TYPE==MOTOR_BLUESHARK_A1
 	m_params.offset = 0.0f; //编码器做了零位置校准
 #endif
 	mc_err_block_init();
-	//sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
+	sys_debug("%f -- %f, flux: %f, %d\n", foc_params.n_currentBand, m_params.ld, m_params.flux_linkage, sizeof(m_params));
 	sys_debug("mc err %d, %d\n", _mc_err_r_id, _mc_err_r_idx);
 }
 

Applications/bsp/gd32/board_mc105_v3.h

@@ -35,7 +35,7 @@
 //#define CONFIG_SENSORLESS_TOW_SAMPLES
 //#define CONFIG_SMO_OBSERVER 
 #define CONFIG_LADRC_OBSERVER
-#define CONFIG_SPEED_LADRC  
+//#define CONFIG_SPEED_LADRC  
 //#define CONFIG_FORCE_96V_MODE
 #ifdef CONFIG_SENSORLESS_TOW_SAMPLES
 #define CONFIG_SENSORLESS_TS (FOC_CTRL_US/2.0f)

Applications/foc/core/PI_Controller.h

@@ -39,6 +39,12 @@ typedef struct {
 	float  b_dstart;
 }PI_Controller;
 
+static __INLINE void PI_Controller_Change_Kpi(PI_Controller *pi, float kp, float ki) {
+	pi->kp = kp;
+	pi->ki = ki;
+}
+
+
 static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min) {
 	pi->max = max;
 	pi->min = min;
@@ -141,18 +147,24 @@ static __INLINE float PI_Controller_Current(PI_Controller *pi, float err, float
 
 
 static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
-	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);
+	float kp_err = (err) * pi->kp;
 	float ki_err = (kp_err) * pi->ki;
 	float integral = ki_err * pi->DT;
-	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
+
+	pi->Ui = pi->Ui + integral;
 	float out = pi->Ui + kp_err;
-	float sat_out =  (MATH_sat(out, pi->min, pi->max));
-	if (out != sat_out) {
+	if (out > pi->max) {
+		out = pi->max;
+		pi->is_sat = true;
+		pi->Ui = out - kp_err;
+	}else if (out < pi->min) {
+		out = pi->min;
 		pi->is_sat = true;
+		pi->Ui = out - kp_err;
 	}else {
 		pi->is_sat = false;
 	}
-	return sat_out;
+	return out;
 }
 
 static __INLINE float _fmod(float v, s32 m) {

Applications/foc/core/PMSM_FOC_Core.c

@@ -126,8 +126,8 @@ static void PMSM_FOC_Reset_PID(void) {
 	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);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_vel, 0);
+	PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, 0);
 #endif
 }
 
@@ -157,14 +157,14 @@ static void PMSM_FOC_Conf_PID(void) {
 	ladrc_init(&gFoc_Ctrl.vel_lim_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_lim_Wo, nv_get_foc_params()->f_adrc_vel_lim_Wcv, nv_get_foc_params()->f_adrc_vel_lim_B0);
 	ladrc_init(&gFoc_Ctrl.vel_adrc, slow_ctrl_ts, nv_get_foc_params()->f_adrc_vel_Wo, nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_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 = slow_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 = slow_ctrl_ts;
+	gFoc_Ctrl.pi_vel_lim.kp = nv_get_foc_params()->pid_conf[PID_TRQ_id].kp;
+	gFoc_Ctrl.pi_vel_lim.ki = nv_get_foc_params()->pid_conf[PID_TRQ_id].ki;
+	gFoc_Ctrl.pi_vel_lim.kd = nv_get_foc_params()->pid_conf[PID_TRQ_id].kd;
+	gFoc_Ctrl.pi_vel_lim.DT = slow_ctrl_ts;
+	gFoc_Ctrl.pi_vel.kp = nv_get_foc_params()->pid_conf[PID_Spd_id].kp;
+	gFoc_Ctrl.pi_vel.ki = nv_get_foc_params()->pid_conf[PID_Spd_id].ki;
+	gFoc_Ctrl.pi_vel.kd = nv_get_foc_params()->pid_conf[PID_Spd_id].kd;
+	gFoc_Ctrl.pi_vel.DT = slow_ctrl_ts;
 #endif
 }
 
@@ -539,23 +539,19 @@ u8 PMSM_FOC_CtrlMode(void) {
 			//ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, 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_vel_lim, gFoc_Ctrl.in.s_targetTorque);
 #endif
 		}else if ((preMode == CTRL_MODE_TRQ) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD)) {
 #ifdef CONFIG_SPEED_LADRC
 			ladrc_copy(&gFoc_Ctrl.vel_adrc, &gFoc_Ctrl.vel_lim_adrc);
 #else
-			float target_troque = gFoc_Ctrl.in.s_targetTorque;
-			if (gFoc_Ctrl.pi_id.is_sat || gFoc_Ctrl.pi_iq.is_sat) {
-				target_troque = PMSM_FOC_Get_Real_dqVector();
-			}
-			PI_Controller_Reset(&gFoc_Ctrl.pi_speed, target_troque);
+			PI_Controller_Reset(&gFoc_Ctrl.pi_vel, gFoc_Ctrl.in.s_targetTorque);
 #endif
 		}else if ((preMode == CTRL_MODE_CURRENT) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_TRQ)) {
 #ifdef CONFIG_SPEED_LADRC
 			ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, gFoc_Ctrl.in.s_motVelocity, gFoc_Ctrl.in.s_targetTorque);
 #else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, gFoc_Ctrl.in.s_targetTorque);
+			PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, gFoc_Ctrl.in.s_targetTorque);
 #endif
 		}else if ((preMode != gFoc_Ctrl.out.n_RunMode) && (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_EBRAKE)) {
 			eCtrl_reset_Torque(gFoc_Ctrl.in.s_targetTorque);
@@ -564,7 +560,7 @@ u8 PMSM_FOC_CtrlMode(void) {
 #ifdef CONFIG_SPEED_LADRC
 			ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, F_get_air());
 #else
-			PI_Controller_Reset(&gFoc_Ctrl.pi_torque, F_get_air());
+			PI_Controller_Reset(&gFoc_Ctrl.pi_vel, F_get_air());
 #endif
 		}
 	}
@@ -617,6 +613,7 @@ static void crosszero_step_towards(float *value, float target) {
 
 #define CHANGE_MAX_CNT 3
 static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
+#if 0
 	static int change_cnt = 0;
 	static bool change_done = false;
 	static u32  change_time = 0xFFFFFFFF;
@@ -693,6 +690,7 @@ static __INLINE void PMSM_FOC_VelCtrl_Decide(void) {
 		change_cnt = 0;
 		gFoc_Ctrl.out.empty_load = false;
 	}
+#endif
 }
 
 static __INLINE float PMSM_FOC_Limit_iDC(float maxTrq) {
@@ -711,15 +709,11 @@ static __INLINE float PMSM_FOC_Limit_Speed(float maxTrq) {
 	ladrc_set_range(&gFoc_Ctrl.vel_lim_adrc, 0, maxTrq);
 	return ladrc_run(&gFoc_Ctrl.vel_lim_adrc, lim, gFoc_Ctrl.in.s_motVelocity);
 #else
-#if 1
-	gFoc_Ctrl.pi_torque.max = maxTrq;
-	gFoc_Ctrl.pi_torque.min = 0;
+	gFoc_Ctrl.pi_vel_lim.max = maxTrq;
+	gFoc_Ctrl.pi_vel_lim.min = 0;
 
 	float err = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.rpmLimRamp) - gFoc_Ctrl.in.s_motVelocity;
-	return PI_Controller_RunLimit(&gFoc_Ctrl.pi_torque, err);
-#else
-	return maxTrq;
-#endif
+	return PI_Controller_RunSerial(&gFoc_Ctrl.pi_vel_lim, err);
 #endif
 }
 
@@ -798,20 +792,20 @@ void PMSM_FOC_idqCalc(void) {
 		float maxTrq = ladrc_run(&gFoc_Ctrl.vel_adrc, refSpeed, gFoc_Ctrl.in.s_motVelocity);
 #else
 		if (maxSpeed >= 0) {
-			gFoc_Ctrl.pi_speed.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed.min = -CONFIG_MAX_NEG_TORQUE;
+			gFoc_Ctrl.pi_vel.max = eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
+			gFoc_Ctrl.pi_vel.min = -CONFIG_MAX_NEG_TORQUE;
 		}else if (maxSpeed < 0) {
-			gFoc_Ctrl.pi_speed.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);//gFoc_Ctrl.userLim.s_PhaseCurrLim;
-			gFoc_Ctrl.pi_speed.max = CONFIG_MAX_NEG_TORQUE;
+			gFoc_Ctrl.pi_vel.min = -eRamp_get_intepolation(&gFoc_Ctrl.rtLim.torqueLimRamp);
+			gFoc_Ctrl.pi_vel.max = CONFIG_MAX_NEG_TORQUE;
 		}
 
 		if ((maxSpeed == 0) && (gFoc_Ctrl.in.s_motVelocity < CONFIG_MIN_RPM_EXIT_EBRAKE)) {
-			gFoc_Ctrl.pi_speed.max = 0;
-			gFoc_Ctrl.pi_speed.min = 0; //防止倒转
+			gFoc_Ctrl.pi_vel.max = 0;
+			gFoc_Ctrl.pi_vel.min = 0; //防止倒转
 		}
 		gFoc_Ctrl.in.s_targetRPM = refSpeed;
 		float errRef = refSpeed - gFoc_Ctrl.in.s_motVelocity;
-		float maxTrq = PI_Controller_Run(&gFoc_Ctrl.pi_speed, errRef);
+		float maxTrq = PI_Controller_RunSerial(&gFoc_Ctrl.pi_vel, errRef);
 #endif
 		maxTrq = PMSM_FOC_Limit_iDC(maxTrq);
 		crosszero_step_towards(&gFoc_Ctrl.in.s_targetTorque, maxTrq);
@@ -856,13 +850,21 @@ void PMSM_FOC_Slow_Task(void) {
 }
 
 void PMSM_FOC_Change_VelLoop_Params(float wcv, float b0) {
+#ifdef CONFIG_SPEED_LADRC
 	ladrc_change_b0(&gFoc_Ctrl.vel_adrc, b0);
 	ladrc_change_K(&gFoc_Ctrl.vel_adrc, wcv);
+#else
+	PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel, wcv, b0);
+#endif
 }
 
 void PMSM_FOC_Change_TrqLoop_Params(float wcv, float b0) {
+#ifdef CONFIG_SPEED_LADRC
 	ladrc_change_b0(&gFoc_Ctrl.vel_lim_adrc, b0);
 	ladrc_change_K(&gFoc_Ctrl.vel_lim_adrc, wcv);
+#else
+	PI_Controller_Change_Kpi(&gFoc_Ctrl.pi_vel_lim, wcv, b0);
+#endif
 }
 
 
@@ -1091,8 +1093,8 @@ void PMSM_FOC_Reset_Torque(void) {
 
 bool PMSM_FOC_Set_CruiseSpeed(float rpm) {
 	if (PMSM_FOC_Is_CruiseEnabled()) {
-		if (rpm < CONFIG_MIN_CRUISE_RPM + 20) {
-			rpm = CONFIG_MIN_CRUISE_RPM + 20;
+		if (rpm < CONFIG_MIN_CRUISE_RPM) {
+			rpm = CONFIG_MIN_CRUISE_RPM;
 		}
 		gFoc_Ctrl.in.s_cruiseRPM = min(ABS(rpm), gFoc_Ctrl.userLim.s_motRPMLim)*SIGN(rpm);
 		eRamp_set_step_target(&gFoc_Ctrl.in.cruiseRpmRamp, gFoc_Ctrl.in.s_cruiseRPM, CONFIG_eCTRL_STEP_TS);
@@ -1140,13 +1142,13 @@ void PMSM_FOC_AutoHold(bool lock) {
 #ifdef CONFIG_SPEED_LADRC
 				ladrc_reset(&gFoc_Ctrl.vel_lim_adrc, 0, hold_torque);
 #else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_torque, hold_torque);
+				PI_Controller_Reset(&gFoc_Ctrl.pi_vel_lim, hold_torque);
 #endif
 			}else if (gFoc_Ctrl.out.n_RunMode == CTRL_MODE_SPD) {
 #ifdef CONFIG_SPEED_LADRC
 				ladrc_reset(&gFoc_Ctrl.vel_adrc, 0, hold_torque);
 #else
-				PI_Controller_Reset(&gFoc_Ctrl.pi_speed, hold_torque);
+				PI_Controller_Reset(&gFoc_Ctrl.pi_vel, hold_torque);
 #endif
 			}
 			eCtrl_reset_Torque(hold_torque);
@@ -1171,11 +1173,11 @@ static PI_Controller *_pid(u8 id) {
 		pi = &gFoc_Ctrl.pi_iq;
 	}else if (id == PID_TRQ_id) {
 #ifndef CONFIG_SPEED_LADRC
-		pi = &gFoc_Ctrl.pi_torque;
+		pi = &gFoc_Ctrl.pi_vel_lim;
 #endif
 	}else if (id == PID_Spd_id) {
 #ifndef CONFIG_SPEED_LADRC
-		pi = &gFoc_Ctrl.pi_speed;
+		pi = &gFoc_Ctrl.pi_vel;
 #endif
 	}
 	return pi;

Applications/foc/core/PMSM_FOC_Core.h

@@ -187,8 +187,8 @@ typedef struct {
 	ladrc_t        vel_lim_adrc;
 	ladrc_t        vel_adrc;
 #else
-	PI_Controller pi_torque;
-	PI_Controller pi_speed;
+	PI_Controller pi_vel_lim;
+	PI_Controller pi_vel;
 #endif
 	dq_Rctrl      idq_ctl[2];
 	dq_Rctrl      vdq_ctl[2];	

Applications/foc/foc_config.h

@@ -25,7 +25,7 @@
 #define CONFIG_THROTTLE_MAX_VALUE 4.8F /* 转把最大电压，大于这个值，不给启动 */
 
 #define CONFIG_MIN_CRUISE_RPM 	  1000   /* 能启动定速巡航的最小速度 */
-
+#define CONFIG_CRUISE_EXIT_RPM    700    /* 自动推出定速巡航的最小速度*/
 #define CONFIG_MIN_RPM_FOR_EBRAKE 800 //进入电流回收的最小转速
 #define CONFIG_MIN_RPM_EXIT_EBRAKE 250 //推出电流回收的最小转速
 

Applications/foc/motor/motor.c

@@ -521,15 +521,21 @@ bool mc_start_epm_move(EPM_Dir_t dir, bool is_command) {
 			mc_auto_hold(false);
 		}
 		if (dir == EPM_Dir_Back) {
+#ifdef CONFIG_SPEED_LADRC
 			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPMBK_B0);
+#endif
 		}else {
-			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPM_B0);
+#ifdef CONFIG_SPEED_LADRC
+			PMSM_FOC_Change_VelLoop_Params(CONFIG_LADRC_EPM_Wcv, CONFIG_LADRC_EPM_B0);S
+#endif
 		}
 		PMSM_FOC_TorqueLimit(motor.f_epm_trq);
 		PMSM_FOC_Set_Speed(motor.f_epm_vel);
 	}else {
 		motor.b_epm_cmd_move = false;
+#ifdef CONFIG_SPEED_LADRC
 		PMSM_FOC_Change_VelLoop_Params(nv_get_foc_params()->f_adrc_vel_Wcv, nv_get_foc_params()->f_adrc_vel_B0);
+#endif
 		PMSM_FOC_Set_Speed(0);
 	}
 	cpu_exit_critical(mask);
@@ -1187,7 +1193,7 @@ static bool mc_process_force_running(void) {
 static void mc_process_curise(void) {
 	static bool can_pause_resume = false;
 	if (motor.b_cruise) {
-		if (PMSM_FOC_GetSpeed() < (CONFIG_MIN_CRUISE_RPM - 20)) {
+		if (PMSM_FOC_GetSpeed() < CONFIG_CRUISE_EXIT_RPM) {
 			mc_enable_cruise(false);
 			return;
 		}

Applications/foc/motor/motor_param.h

@@ -61,9 +61,14 @@ float motor_get_ebreak_toruqe(float rpm);
 #define MOTOR_ENC_OFFSET 0.0F
 #define CONFIG_MAX_MOTOR_TORQUE 400.0f//45.0F
 
-#define TRQ_PI_KP 0.05F
-#define TRQ_PI_KI 0.01F
-#define TRO_PI_KD 0.0F
+#define PI_VEL_LIM_KP 0.085F //0.13f
+#define PI_VEL_LIM_KI 2.5F
+#define PI_VEL_LIM_KD 0.0F
+
+#define PI_VEL_KP 0.1F //0.13f
+#define PI_VEL_KI 3.0F
+#define PI_VEL_KD 0.0F
+
 #define MOTOR_NR 0x16
 
 //4000->10, 34; 8000->8, 15
@@ -97,9 +102,13 @@ float motor_get_ebreak_toruqe(float rpm);
 #define MOTOR_ENC_OFFSET 0.0F
 #define CONFIG_MAX_MOTOR_TORQUE 200.0F
 
-#define TRQ_PI_KP 0.6F //0.13f
-#define TRQ_PI_KI 0.5F
-#define TRO_PI_KD 0.0F
+#define PI_VEL_LIM_KP 0.085F //0.13f
+#define PI_VEL_LIM_KI 2.5F
+#define PI_VEL_LIM_KD 0.0F
+
+#define PI_VEL_KP 0.1F //0.13f
+#define PI_VEL_KI 3.0F
+#define PI_VEL_KD 0.0F
 
 #define MOTOR_NR 0x13
 

Applications/prot/can_message.c

@@ -121,9 +121,19 @@ void handle_can_frame(can_id_t id, uint8_t *data, int len){
 	}
 }
 
+static bool _pc_connect = false;
+bool can_is_connect_pc(void) {
+	return _pc_connect;
+}
 
 static void can_process_message(can_message_t *message){
 	//sys_debug("can %x [%x -> %x], len = %d\n", message->key, message->src, message->dest, message->len);
+	if (message->src == 0x45) {
+		if (!_pc_connect) {
+			set_log_level(MOD_SYSTEM, L_debug);
+		}
+		_pc_connect = true;
+	}
 	if ((message->key & 0xFF) >= 0xF0) {
 		can_process_iap_message(message);
 	}else if ((message->key & 0xFF) >= 0xE0) { //工厂测试

Applications/prot/can_message.h

@@ -57,5 +57,6 @@ extern void can_send_ack(uint8_t can_addr, uint16_t key, uint8_t data);
 extern void can_send_response(uint8_t can_add, uint8_t *data, int len);
 extern void can_send_indicator(uint8_t can_add, uint8_t *data, int len);
 extern void can_send_request(uint8_t can_add, uint8_t *data, int len);
+extern bool can_is_connect_pc(void);
 #endif /* _CAN_MESSAGE_H__ */