From 203a25e7a7807bf6c3eb7c81272894298f48c321 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B5=B7=E5=BA=8A=E5=B0=B1=E7=8A=AF=E5=9B=B0?= <11730503+psx123456@user.noreply.gitee.com> Date: Fri, 30 May 2025 16:36:53 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=94=B9=E9=80=9F=E5=BA=A6=E8=AE=A1?= =?UTF-8?q?=E7=AE=97=E6=96=B9=E5=BC=8F=E5=92=8C=E7=9B=B8=E7=94=B5=E6=B5=81?= =?UTF-8?q?=E8=AE=A1=E7=AE=97=E6=96=B9=E5=BC=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 6 ++ APP/Device/Device_speed/speed_to_bldc.c | 103 ++++++++++++++++++++---- APP/Device/Device_speed/speed_to_bldc.h | 16 ++-- APP/Service/service_statusmonitor.c | 30 ++++--- PROJECT/OS2.ewp | 8 +- 5 files changed, 128 insertions(+), 35 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..8da1df1 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,6 @@ +{ + "files.associations": { + "bsp_os.h": "c", + "cmath": "c" + } +} \ No newline at end of file diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c index 913f112..3fec97d 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -9,6 +9,10 @@ #ifdef PTZ_BLDC_MOTOR +static volatile float tempSpeedH = 0; +static volatile float tempSpeedV = 0; + + //发送云台实际转速 void ptz_send_speed(char dev, char speed) { @@ -64,9 +68,9 @@ void ptz_pid_init() g_ptz.hori_pid.TD = PTZ_HORI_PID_HORI_TD;//微分常数 g_ptz.hori_pid.T = PTZ_HORI_PID_T / 1000.0;//采样周期 //三个简化参数A，B，C - g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD); - g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T); - g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T); +// g_ptz.hori_pid.A = g_ptz.hori_pid.KP * (1 + g_ptz.hori_pid.T / g_ptz.hori_pid.TI + g_ptz.hori_pid.TD); +// g_ptz.hori_pid.B = g_ptz.hori_pid.KP * (1 + 2 * g_ptz.hori_pid.TD / g_ptz.hori_pid.T); +// g_ptz.hori_pid.C = g_ptz.hori_pid.KP * (g_ptz.hori_pid.TD / g_ptz.hori_pid.T); g_ptz.vert_pid.SumError = 0;//累计误差 g_ptz.vert_pid.LastError = 0;//上一次输入偏差 @@ -78,9 +82,10 @@ void ptz_pid_init() g_ptz.vert_pid.TD = PTZ_VERT_PID_VERT_TD;//微分常数 g_ptz.vert_pid.T = PTZ_VERT_PID_T / 1000.0;//采样周期 //三个简化参数A，B，C - g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD); - g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T); - g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T); +// g_ptz.vert_pid.A = g_ptz.vert_pid.KP * (1 + g_ptz.vert_pid.T / g_ptz.vert_pid.TI + g_ptz.vert_pid.TD); +// g_ptz.vert_pid.B = g_ptz.vert_pid.KP * (1 + 2 * g_ptz.vert_pid.TD / g_ptz.vert_pid.T); +// g_ptz.vert_pid.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T); + } void ptz_hori_pid_clear_zero() @@ -108,7 +113,9 @@ static float ptz_hori_pid_calculate(float H_SampSpeed) // H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1; // } //增量计算 - H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError; +// H_IIncPid = g_ptz.hori_pid.A * H_IError + g_ptz.hori_pid.B * g_ptz.hori_pid.LastError + g_ptz.hori_pid.C * g_ptz.hori_pid.PrevError; + H_IIncPid = g_ptz.hori_pid.KP * (H_IError - g_ptz.hori_pid.LastError) + g_ptz.hori_pid.TI * H_IError; + //存储误差，用于下次计算 g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; g_ptz.hori_pid.LastError = H_IError; @@ -121,6 +128,10 @@ static float ptz_hori_pid_calculate(float H_SampSpeed) //新调速方式 static void ptz_hori_pid_task() { + + + OSTimeDlyHMSM(0u, 0u, 0u, 1000u); + unsigned int time = 0; char i = 0; while(1) @@ -580,7 +591,36 @@ static void ptz_hori_pid_task() g_ptz.hori_speed_actual = g_ptz.hori_speed_hall_actual; #endif - + +#ifdef PTZ_PID_AS5047_SPEED + + static float lastAngle = 0; + static float lastTime = 0; + float angle = 0; + float time = 0; + + OSTimeDlyHMSM(0u, 0u, 0u, 20u); +/* 读取到正确速度为止 */ +start: + angle = as5047d_hori_get_angle_a(); + time = OSTimeGet(); + if (angle < 0 || //防止读出的角度为负值 + angle > 360.0||//防止读出的数值超过360 + isnan(angle) == 1) {//防止读出的不是一个数 + //以上错误的数据都该舍弃,从新读取 + goto start; + } + + float angleDifference = fabs(angle - lastAngle); + if (angleDifference < 3) { + // goto start; + return; + } + + float timeDifference = fabs(time - lastTime); + g_ptz.hori_speed_actual = angleDifference / timeDifference / PTZ_HORI_RATIO * 1000; + +#endif /*由于系统实时调节能力不足将调速分为两个部分： 1.启动时首先进入直线比例快速提速； @@ -645,7 +685,9 @@ static void ptz_hori_pid_task() { g_ptz.hori_pid.PidUT_float = PTZ_HORI_VR_MAX; } - + + tempSpeedH = g_ptz.hori_pid.PidUT_float; + g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5); //限制PID的输出值在某个指定的范围 @@ -754,7 +796,9 @@ static float ptz_vert_pid_calculate(float SampSpeed) // IError = PTZ_VERT_PID_INPUT_LIMIT * -1; // } //增量计算 - IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError; +// IIncPid = g_ptz.vert_pid.A * IError + g_ptz.vert_pid.B * g_ptz.vert_pid.LastError + g_ptz.vert_pid.C * g_ptz.vert_pid.PrevError; + IIncPid = g_ptz.vert_pid.KP * (IError - g_ptz.vert_pid.LastError) + g_ptz.vert_pid.TI * IError; + //存储误差，用于下次计算 g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.LastError = IError; @@ -1196,7 +1240,7 @@ static void ptz_vert_pid_task() g_ptz.vert_pid.hall_h123_motor_speed = 60000.0 * (float)(g_ptz.vert_pid.hall_h123_count /*-1*/) / (float)PTZ_VERT_PID_T / PTZ_VERT_ONE_CYCLE_HALL_NUM; - + } g_ptz.vert_motor_speed_hall_actual = g_ptz.vert_pid.hall_h123_motor_speed; @@ -1216,10 +1260,37 @@ static void ptz_vert_pid_task() //根据电机转速计算云台转速 g_ptz.vert_speed_hall_actual = g_ptz.vert_motor_speed_hall_actual / PTZ_VERT_RATIO; g_ptz.vert_speed_actual = g_ptz.vert_speed_hall_actual; - + #endif - - + +#ifdef PTZ_PID_AS5047_SPEED + static float lastAngle = 0; + static float lastTime = 0; + float angle = 0; + float time = 0; + + OSTimeDlyHMSM(0u, 0u, 0u, 20u); +/* 读取到正确速度为止 */ +start: + angle = as5047d_vert_get_angle_a(); + time = OSTimeGet(); + if (angle < 0 || //防止读出的角度为负值 + angle > 360.0||//防止读出的数值超过360 + isnan(angle) == 1) {//防止读出的不是一个数 + //以上错误的数据都该舍弃,从新读取 + goto start; + } + + float angleDifference = fabs(angle - lastAngle); + if (angleDifference < 3) { + // goto start; + return; + } + + float timeDifference = fabs(time - lastTime); + g_ptz.vert_speed_actual = angleDifference / timeDifference / PTZ_VERT_RATIO * 166.667f; +#endif + } //PID调速 @@ -1303,7 +1374,9 @@ static void ptz_vert_pid_task() } g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5); - + + tempSpeedV = g_ptz.vert_pid.PidUT_float; + //限制PID的输出值在某个指定的范围 if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值 { diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index 5d28824..a5d5c79 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -228,23 +228,25 @@ //通过芯片反馈测速 //#define PTZ_HORI_PID_JY02A_SPEED //#define PTZ_VERT_PID_JY02A_SPEED - #define PTZ_PID_HALL_SPEED 1 + // #define PTZ_PID_HALL_SPEED 1 + + #define PTZ_PID_AS5047_SPEED 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 20.0//比例系数 - #define PTZ_HORI_PID_HORI_TI 100.0 //积分系数 + #define PTZ_HORI_PID_HORI_KP 100.0//比例系数 + #define PTZ_HORI_PID_HORI_TI 30.0 //积分系数 #define PTZ_HORI_PID_HORI_TD 0.0 //微分系数 #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID调速输入值限定 - #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 + #define PTZ_HORI_PID_OUTPUT_LIMIT 50.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 #define PTZ_VERT_PID_T 30u - #define PTZ_VERT_PID_VERT_KP 20.0//比例系数 - #define PTZ_VERT_PID_VERT_TI 100.0 //积分系数 + #define PTZ_VERT_PID_VERT_KP 100.0//比例系数 + #define PTZ_VERT_PID_VERT_TI 30.0 //积分系数 #define PTZ_VERT_PID_VERT_TD 0.0 //微分系数 #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定 - #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 + #define PTZ_VERT_PID_OUTPUT_LIMIT 50.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 #define PTZ_HORI_PID_T_MAX 100u//PID调速最大周期 #define PTZ_VERT_PID_T_MAX 100u//PID调速最大周期 diff --git a/APP/Service/service_statusmonitor.c b/APP/Service/service_statusmonitor.c index c727ad5..33d5d52 100644 --- a/APP/Service/service_statusmonitor.c +++ b/APP/Service/service_statusmonitor.c @@ -14,23 +14,35 @@ static char ptz_temp_volt_current_fault_detect_task() static unsigned short int time_ms; time_ms ++; + + static unsigned char hCurrCount = 0; + static unsigned char vCurrCount = 0; + + if(time_ms < 50) { //轻型云台峰值电流5.4A， if(H_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||H_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 - - g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警 - - ptz_hori_stop(PTZ_HORI_STOP_TIME); + if (3 < hCurrCount++) { + g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警 + ptz_hori_stop(PTZ_HORI_STOP_TIME); + } } + else + hCurrCount = 0; + if(V_ADC_Collect.Phase_curr_V >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_U >= PHASE_CURRENT ||V_ADC_Collect.Phase_curr_W >= PHASE_CURRENT ) - {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 - - g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警 - - ptz_vert_stop(PTZ_VERT_STOP_TIME); + {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 + if (3 < vCurrCount++) { + g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警 + ptz_vert_stop(PTZ_VERT_STOP_TIME); + } } + else + vCurrCount = 0; + + return 1; } else diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp index c85f083..68acab8 100644 --- a/PROJECT/OS2.ewp +++ b/PROJECT/OS2.ewp @@ -718,11 +718,11 @@