kb 改为 kd，不使用饱和系数，改为微分系数

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

Applications/app/nv_storage.c

@@ -68,31 +68,31 @@ static void nv_default_foc_params(void) {
 	foc_params.f_maxThroVol = CONFIG_THROTTLE_MAX_VALUE;
 	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_D_id].kd = 0;
 	
 	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_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].kb = 1.0f;
+	foc_params.pid_conf[PID_TRQ_id].kd = TRO_PI_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].kb = 1.0f;
+	foc_params.pid_conf[PID_Spd_id].kd = 0.0f;
 
 	foc_params.pid_conf[PID_Pow_id].kp = 5.0f;
 	foc_params.pid_conf[PID_Pow_id].ki = 15.0f;
-	foc_params.pid_conf[PID_Pow_id].kb = 0;
+	foc_params.pid_conf[PID_Pow_id].kd = 0;
 
 	foc_params.pid_conf[PID_Lock_id].kp = (0.01f);
 	foc_params.pid_conf[PID_Lock_id].ki = (0.20f);
-	foc_params.pid_conf[PID_Lock_id].kb = 0;
+	foc_params.pid_conf[PID_Lock_id].kd = 0;
 
 	foc_params.pid_conf[PID_FW_id].kp = 0;
 	foc_params.pid_conf[PID_FW_id].ki = 0.01f;
-	foc_params.pid_conf[PID_FW_id].kb = 0;
+	foc_params.pid_conf[PID_FW_id].kd = 0;
 }
 
 static void nv_default_gear_config(void) {

Applications/foc/commands.c

@@ -176,6 +176,19 @@ static void process_foc_command(foc_cmd_body_t *command) {
 		#endif
 			break;
 		}
+		case Foc_Set_Gear_Mode:
+		{
+			u8 gear = decode_u8(command->data);
+			if (gear > 3) {
+				erroCode = FOC_Param_Err;
+			}else {
+				sys_debug("set gear %d\n", gear);
+				mc_set_gear(gear);
+				response[3] = gear;
+				len += 1;
+			}
+			break;
+		}
 		case Foc_Set_Cruise_Mode:
 		{
 			u8 enable = decode_u8(command->data);
@@ -310,8 +323,8 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			pid_conf_t pid;
 			u8 id =   decode_u8((u8 *)command->data);
 			memcpy((char *)&pid, (char *)command->data + 1, sizeof(pid_conf_t));
-			sys_debug("set 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);
+			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
+			PMSM_FOC_SetPid(id, pid.kp, pid.ki, pid.kd);
 			nv_set_pid(id, &pid);
 			break;
 		}
@@ -324,7 +337,7 @@ static void process_foc_command(foc_cmd_body_t *command) {
 				erroCode = id;
 				memcpy(response+3, &pid, sizeof(pid));
 				len = sizeof(pid) + 3;
-				sys_debug("get id = %d, kp = %f, ki = %f, kb = %f\n", id, pid.kp, pid.ki, pid.kb);
+				sys_debug("get id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
 			}else {
 				erroCode = 1;
 				len = 3;

Applications/foc/commands.h

@@ -41,6 +41,7 @@ typedef enum {
 	Foc_SN_Write, //66
 	Foc_SN_Read,
 	Foc_Set_DQ_Current, //0x44
+	Foc_Set_Gear_Mode,//0-3, eco, normal, sport, turbo
 	Foc_Hall_Phase_Cali_Result = 160,
 	Foc_Hall_Offset_Cali_Result,
 	Foc_Report_Speed,
@@ -91,7 +92,7 @@ typedef struct {
 typedef struct {
 	float kp;
 	float ki;
-	float kb;
+	float kd;
 }pid_conf_t;
 
 #pragma pack(pop)

Applications/foc/core/PI_Controller.h

@@ -28,13 +28,15 @@ static __INLINE s16q5_t PI_Controller_run(PI_Controller *pi, s16q5_t err) {
 typedef struct {
 	float  kp;
 	float  ki;
-	float  kb;
+	float  kd;
 	float  max;
 	float  min;
 	float  Ui;
 	float  sat;
 	float  DT;
 	bool   is_sat;
+	float  last_err;
+	float  b_dstart;
 }PI_Controller;
 
 static __INLINE void PI_Controller_max(PI_Controller *pi, float max, float min) {
@@ -45,6 +47,7 @@ static __INLINE void PI_Controller_Reset(PI_Controller *pi, float init) {
 	pi->Ui = (init);
 	pi->sat = 0.0f;
 	pi->is_sat = false;
+	pi->b_dstart = false;
 }
 
 static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
@@ -62,11 +65,33 @@ static __INLINE float PI_Controller_Run(PI_Controller *pi, float err) {
 	return sat_out;
 }
 
+static __INLINE float PI_Controller_RunWithDiff(PI_Controller *pi, float err) {
+	float kp_err = (err) * pi->kp;
+	float ki_err = (err) * pi->ki;
+	float integral = ki_err * pi->DT;
+	if (!pi->b_dstart) {
+		pi->last_err = err;
+		pi->b_dstart = true;
+	}
+	
+	pi->Ui = MATH_sat(pi->Ui + integral, pi->min, pi->max);
+	float out = pi->Ui + kp_err + pi->kd * (err - pi->last_err);
+	float sat_out =  (MATH_sat(out, pi->min, pi->max));
+	if (out != sat_out) {
+		pi->is_sat = true;
+	}else {
+		pi->is_sat = false;
+	}
+	pi->last_err = err;
+	return sat_out;
+}
+
+#if 0
 static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;
 	float ki_err = (err) * pi->ki;
 	float integral = ki_err * pi->DT;
-	pi->Ui = pi->Ui + integral + pi->sat * pi->kb;
+	pi->Ui = pi->Ui + integral + pi->sat * pi->kd;
 	float out = pi->Ui + kp_err ;
 	float out_sat = MATH_sat(out, pi->min, pi->max);
 	if (out != out_sat) {
@@ -78,7 +103,7 @@ static __INLINE float PI_Controller_RunSat(PI_Controller *pi, float err) {
 
 	return out_sat;
 }
-
+#endif
 
 static __INLINE float PI_Controller_RunSerial(PI_Controller *pi, float err) {
 	float kp_err = (err) * pi->kp;//S16_mul(err,pi->kp, 5);

Applications/foc/core/PMSM_FOC_Core.c

@@ -128,37 +128,37 @@ static void PMSM_FOC_Reset_PID(void) {
 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->kb = nv_get_foc_params()->pid_conf[PID_D_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_Q_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_TRQ_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_Spd_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_Pow_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_Lock_id].kb;
+	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->kb = nv_get_foc_params()->pid_conf[PID_FW_id].kb;
+	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);
 }
@@ -525,7 +525,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;
-	gFoc_Ctrl.params.f_DCLim = PI_Controller_Run(gFoc_Ctrl.pi_torque, err);
+	gFoc_Ctrl.params.f_DCLim = PI_Controller_RunWithDiff(gFoc_Ctrl.pi_torque, err);
 	return maxTrq;
 #else
 	gFoc_Ctrl.params.f_DCLim = gFoc_Ctrl.in.s_vDC;
@@ -962,7 +962,7 @@ static PI_Controller *_pid(u8 id) {
 	return pi;
 }
 
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd) {
 	if (id > PID_Max_id) {
 		return;
 	}
@@ -970,11 +970,11 @@ void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb) {
 	if (pi != NULL) {
 		pi->kp = kp;
 		pi->ki = ki;
-		pi->kb = kb;
+		pi->kd = kd;
 	}
 }
 
-void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb) {
+void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd) {
 	if (id > PID_Max_id) {
 		return;
 	}
@@ -982,7 +982,7 @@ void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb) {
 	if (pi != NULL) {
 		*kp = pi->kp;
 		*ki = pi->ki;
-		*kb = pi->kb;
+		*kd = pi->kd;
 	}
 }
 

Applications/foc/core/PMSM_FOC_Core.h

@@ -262,7 +262,7 @@ bool PMSM_FOC_Lock_Motor(bool lock);
 void PMSM_FOC_Brake(bool brake);
 void PMSM_FOC_Calc_Current(void);
 void PMSM_FOC_AutoHold(bool lock);
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
 void PMSM_FOC_Set_Angle(float angle);
 bool PMSM_FOC_Is_Start(void);
 void PMSM_FOC_SetErrCode(u8 error);
@@ -282,8 +282,8 @@ float PMSM_FOC_GetPhaseCurrLim(void);
 float PMSM_FOC_GetDCCurrLimit(void);
 void PMSM_FOC_GetRunningStatus(u8 *data);
 bool PMSM_FOC_Is_CruiseEnabled(void);
-void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kb);
-void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kb);
+void PMSM_FOC_SetPid(u8 id, float kp, float ki, float kd);
+void PMSM_FOC_GetPid(u8 id, float *kp, float *ki, float *kd);
 bool PMSM_FOC_AutoHoldding(void);
 void PMSM_FOC_Slow_Task(void);
 void PMSM_FOC_Set_PlotType(Plot_t t);

Applications/foc/core/PMSM_FOC_Params.h

@@ -27,7 +27,7 @@ static PI_Controller PI_Ctrl_IQ = {
 static PI_Controller PI_Ctrl_trq = {
 	//.kp = 0.1f,
 	//.ki = 0.5f,
-	//.kb = 1.2f,
+	//.kd = 1.2f,
 	.max = (0),
 	.min = (0),
 	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),
@@ -37,7 +37,7 @@ static PI_Controller PI_Ctrl_trq = {
 static PI_Controller PI_Ctrl_Spd = {
 	//.kp = 0.1f,
 	//.ki = 0.5f,
-	//.kb = 1.2f,
+	//.kd = 1.2f,
 	.max = (0),
 	.min = (-0),
 	.DT  = (1.0f/(float)CONFIG_SPD_CTRL_TS),

Applications/foc/motor/motor_param.h

@@ -34,9 +34,9 @@ s16 get_max_torque_for_rpm(s16 rpm);
 #define MOTOR_ENC_OFFSET 0.0F
 #define CONFIG_MAX_MOTOR_TORQUE 200.0F
 
-#define TRQ_PI_KP 0.14F
-#define TRQ_PI_KI 0.15F
-
+#define TRQ_PI_KP 0.08F
+#define TRQ_PI_KI 0.1F
+#define TRO_PI_KD 1.0F
 #define MOTOR_NR 0x16
 
 #define CONFIG_CURRENT_BANDWITH  2000.0f /* 电流环带宽 */

Applications/prot/can_foc_msg.c

@@ -67,14 +67,14 @@ void can_response_hall_offset(u8 can, int offset) {
 }
 
 void can_report_pid_value(u8 can, u8 id) {
-	float kp, ki, kb;
-	PMSM_FOC_GetPid(id, &kp, &ki, &kb);
+	float kp, ki, kd;
+	PMSM_FOC_GetPid(id, &kp, &ki, &kd);
 	u8 data[15];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Pid));
 	data[2] = id;
 	encode_float(data + 3, kp);
 	encode_float(data + 7, ki);
-	encode_float(data + 11, kb);
+	encode_float(data + 11, kd);
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }