加入epm模式pid参数可配置

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

Applications/foc/commands.c

@@ -346,15 +346,17 @@ static void process_foc_command(foc_cmd_body_t *command) {
 			u8 id = decode_u8((u8 *)command->data);
 			mc_conf_decode_pid(&pid, (u8 *)command->data + 1);
 			sys_debug("set id = %d, kp = %f, ki = %f, kd = %f\n", id, pid.kp, pid.ki, pid.kd);
-			mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
 			mc_conf_set_pid(id, &pid);
+			if (id < PID_Max_ID) {
+				mot_contrl_set_pid(&motor.controller, id, pid.kp, pid.ki, pid.kd);
+			}
 			break;
 		}
 		case Foc_Get_Pid:
 		{
 			pid_t pid;
 			u8 id =   decode_u8((u8 *)command->data);
-			if (id < PID_Max_ID) {
+			if ((id < PID_Max_ID) || (id == PID_EPM_ExtID)) {
 				mc_conf_get_pid(id, &pid);
 				erroCode = id;
 				len += mc_conf_encode_pid(&pid, response + 3);

Applications/foc/core/controller.c

@@ -771,9 +771,11 @@ void mot_contrl_set_pid(mot_contrl_t *ctrl, u8 id, float kp, float ki, float kd)
 	}
 	PI_Controller *pi = _pid(ctrl, id);
 	if (pi != NULL) {
+		u32 mask = cpu_enter_critical();
 		pi->kp = kp;
 		pi->ki = ki;
 		pi->kd = kd;
+		cpu_exit_critical(mask);
 	}
 }
 

Applications/foc/mc_config.c

@@ -288,6 +288,7 @@ void mc_conf_init(void) {
 		mc_conf_default();
 		mc_conf_save();
 	}
+	sys_debug("mc size %d - %d\n", sizeof(conf), sizeof(conf.c));
 	sys_debug("mc conf band %d, pid: %f, %f, %f, %f, %d\n", conf.c.dq_loop_bandwith, conf.c.pid[PID_ID_ID].kp, conf.c.pid[PID_ID_ID].ki, conf.c.pid[PID_IQ_ID].kp, conf.c.pid[PID_IQ_ID].ki, mc_conf()->m.encoder_offset);
 }
 
@@ -373,6 +374,7 @@ int mc_conf_decode_controler(u8 *buff) {
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_Vel_ID], buff);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
 	buff += mc_conf_decode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
+	buff += mc_conf_decode_pid(&conf.c.epm_pid, buff);
 
 	conf.c.pid[PID_ID_ID].kp = (float)conf.c.dq_loop_bandwith * 2.0f * 3.14f * conf.m.ld;
 	conf.c.pid[PID_ID_ID].ki = conf.m.rs / conf.m.ld;
@@ -409,6 +411,7 @@ int mc_conf_encode_controler(u8 *buff) {
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_Vel_ID], buff);
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_AutoHold_ID], buff);
 	buff += mc_conf_encode_pid(&conf.c.pid[PID_IDCLim_ID], buff);
+	buff += mc_conf_encode_pid(&conf.c.epm_pid, buff);
 	return buff - ori;
 }
 
@@ -559,11 +562,19 @@ int mc_conf_encode_configs(u8 *buff) {
 }
 
 void mc_conf_set_pid(u8 id, pid_t *pid) {
-	conf.c.pid[id] = *pid;
+	if (id < PID_Max_ID) {
+		conf.c.pid[id] = *pid;
+	}else if (id == PID_EPM_ExtID){
+		conf.c.epm_pid = *pid;
+	}
 }
 
 void mc_conf_get_pid(u8 id, pid_t *pid) {
-	*pid = conf.c.pid[id];
+	if (id < PID_Max_ID) {
+		*pid = conf.c.pid[id];
+	}else if (id == PID_EPM_ExtID){
+		*pid = conf.c.epm_pid;
+	}
 }
 
 bool mc_conf_set_gear(u8 mode, u8 *data, int len) {

Applications/foc/mc_config.h

@@ -16,7 +16,8 @@ typedef enum {
 	PID_VelLim_ID,
 	PID_IDCLim_ID,
 	PID_AutoHold_ID,
-	PID_Max_ID
+	PID_Max_ID,
+	PID_EPM_ExtID,
 }PID_id_t;
 
 typedef struct
@@ -63,6 +64,8 @@ typedef struct {
 	float thro_max_vol;
 	u16   thro_dec_time;
 	pid_t pid[PID_Max_ID];
+	pid_t epm_pid;
+	u8    _pad[64];
 }controller_t;
 
 typedef struct {

Applications/foc/motor/motor.c

@@ -730,11 +730,7 @@ bool mc_start_epm_move(epm_dir_t dir, bool is_command) {
 		}else if (mot_contrl_is_auto_holdding(&motor.controller)) {
 			mc_auto_hold(false);
 		}
-		if (dir == EPM_Dir_Back) {
-			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
-		}else {
-			mot_contrl_velloop_params(&motor.controller, 0.2f, 7.5f);
-		}
+		mot_contrl_velloop_params(&motor.controller, mc_conf()->c.epm_pid.kp, mc_conf()->c.epm_pid.ki);
 		mot_contrl_set_torque_limit_rttime(&motor.controller, 2.0);
 		mot_contrl_set_torque_limit(&motor.controller, motor.f_epm_trq);
 		mot_contrl_set_vel_rttime(&motor.controller, 1000.0f);

Applications/prot/can_foc_msg.c

@@ -79,18 +79,6 @@ void can_response_hall_offset(u8 can, int offset) {
 	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
 }
 
-void can_report_pid_value(u8 can, u8 id) {
-	float kp, ki, kd;
-	mot_contrl_get_pid(&motor.controller, 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, kd);
-	can_send_message(get_indicator_can_id(can), data, sizeof(data), 0);
-}
-
 void can_report_foc_status(u8 can) {
 	u8 data[16];
 	encoder_can_key(data, CMD_2_CAN_KEY(Foc_Report_Status));

Applications/prot/can_foc_msg.h

@@ -8,7 +8,6 @@ void can_report_phase_voltage(u8 can);
 void can_report_dq_current(u8 can);
 void can_response_hall_offset(u8 can, int offset);
 void can_report_power(u8 can);
-void can_report_pid_value(u8 can, u8 id);
 void can_report_foc_status(u8 can);
 void can_report_mpta_values(u8 can);
 void can_report_ext_status(u8 can);

configs/mc105_A1.yml

@@ -52,6 +52,10 @@ Foc:
       Kp: 5
       Ki: 15
       Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1

configs/mc124_R1.yml

@@ -1,4 +1,4 @@
-##### 配置文件自动生成，不要手动修改!! 2023/10/27 17:43:29
+##### 配置文件自动生成，不要手动修改!! 2023/11/2 16:25:58
 Version: 1
 CheckCrc: 0
 Motor:
@@ -17,8 +17,8 @@ Motor:
   MaxTorque: 350
   EncOffset: 0
 Foc:
-  MaxDCVol: 95
-  MinDCVol: 60
+  MaxDCVol: 90
+  MinDCVol: 40
   MaxPhaseCurr: 350
   MaxRPM: 9000
   MaxEPMRPM: 300
@@ -29,16 +29,16 @@ Foc:
   MaxEbrkTorque: 40
   MaxIDC: 100
   MaxAutoHoldTorque: 100
-  ThroStartVol: 0.85
-  ThroEndVol: 4.15
+  ThroStartVol: 0.9
+  ThroEndVol: 4
   ThroMinVol: 0.4
   ThroMaxVol: 4.6
   CurrCtrlBandWith: 800
-  ThroDecTime: 10
+  ThroDecTime: 50
   PID:
     VelLim:
-      Kp: 0.5
-      Ki: 2.5
+      Kp: 0.1
+      Ki: 3.5
       Kd: 0
     VelCtrl:
       Kp: 0.1
@@ -52,12 +52,16 @@ Foc:
       Kp: 5
       Ki: 15
       Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1
-  TcsEnable: 0
+  TcsEnable: 1
 Gear:
-- MaxSpeed: 5000
+- MaxSpeed: 2000
   MaxTorque: 100
   MaxIdc: 30
   ZeroAccl: 500
@@ -66,27 +70,27 @@ Gear:
   - 100
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
   - 0
   - 0
-- MaxSpeed: 1000
-  MaxTorque: 100
-  MaxIdc: 30
-  ZeroAccl: 500
-  NormalAccl: 100
+  - 0
+  - 0
+  - 0
+  - 0
+  - 0
+- MaxSpeed: 2800
+  MaxTorque: 200
+  MaxIdc: 35
+  ZeroAccl: 300
+  NormalAccl: 80
   Torque:
   - 100
   - 100
   - 100
-  - 100
-  - 100
-  - 100
-  - 100
-  - 100
+  - 80
+  - 0
+  - 0
+  - 0
+  - 0
   - 0
   - 0
 - MaxSpeed: 1000
@@ -205,15 +209,15 @@ Protect:
     Exit: 90
     Value: 34
   - Entry: 90
-    Exit: 80
+    Exit: 85
     Value: 66
   Voltage:
-  - Entry: 57
-    Exit: 60
+  - Entry: 40
+    Exit: 42
     Value: 0
 EnergyRecovery:
-- Torque: 0
-  Time: 1000
+- Torque: 10
+  Time: 600
 - Torque: 10
   Time: 500
 - Torque: 15
@@ -233,7 +237,7 @@ EnergyRecovery:
 - Torque: 40
   Time: 200
 CrossZero:
-  Low: 1
+  Low: 2
   High: 7
-  MinStep: 5
+  MinStep: 0.05
   NorStep: 5

configs/mc_servo.yml

@@ -52,6 +52,10 @@ Foc:
       Kp: 5
       Ki: 15
       Kd: 0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1

configs/mc_zd100.yml

@@ -52,6 +52,10 @@ Foc:
       Kp: 5.0
       Ki: 15.0
       Kd: 0.0
+    EPM:
+      Kp: 0.2
+      Ki: 7.5
+      Kd: 0
 Settings:
   AutoHold: 1
   BrkShutPower: 1