From 08f686831e5fc4c7e4690919c87ee5329f01c93e 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: Tue, 3 Jun 2025 10:33:27 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E7=BC=96=E7=A0=81=E5=99=A8?= =?UTF-8?q?=E9=80=9F=E5=BA=A6=E8=AE=A1=E7=AE=97=E5=87=BA=E9=94=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 +- APP/Device/Device_speed/speed_to_bldc.c | 74 ++++- APP/Device/Device_speed/speed_to_bldc.h | 310 +++++++++--------- BSP/Driver/full_bridge/full_bridge.c | 1 + .../SYSTICK/gd32f4xx_it.c | 4 + 5 files changed, 233 insertions(+), 160 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 8da1df1..2110bc6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,6 +1,8 @@ { "files.associations": { "bsp_os.h": "c", - "cmath": "c" + "cmath": "c", + "service_error_count.h": "c", + "speed_to_step.h": "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 3fec97d..7cb20cc 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -599,7 +599,10 @@ static void ptz_hori_pid_task() float angle = 0; float time = 0; - OSTimeDlyHMSM(0u, 0u, 0u, 20u); + term_printf("OSTimeGet : %d \n", OSTimeGet()); + OSTimeDlyHMSM(0u, 0u, 0u, 10u); + term_printf("OSTimeGet : %d \n", OSTimeGet()); + /* ¶ÁÈ¡µ½ÕýÈ·ËÙ¶ÈΪֹ */ start: angle = as5047d_hori_get_angle_a(); @@ -612,13 +615,22 @@ start: } float angleDifference = fabs(angle - lastAngle); - if (angleDifference < 3) { - // goto start; - return; + if (angleDifference > 180) { + angleDifference = 360 - angleDifference; } + // if (angleDifference < 3) { + // // goto start; + // return; + // } + float timeDifference = fabs(time - lastTime); - g_ptz.hori_speed_actual = angleDifference / timeDifference / PTZ_HORI_RATIO * 1000; + g_ptz.hori_speed_actual = angleDifference / timeDifference / PTZ_HORI_RATIO * 166.67f; + + lastAngle = angle; + lastTime = time; + + term_printf("angleDifference : %f, timeDifference : %f\n", angleDifference, timeDifference); #endif @@ -1391,7 +1403,8 @@ start: //½«PIDÊä³öµÄµç»úתËÙÄ£ÄâµçѹµÈ¼¶ÊäÈ뵽ģÄâµçѹÊä³öоƬ // vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint); - v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); + // v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); + v_pwm_duty_change(500); //½«µ±Ç°PIDÊä³öÖµ±£´æ g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float; } @@ -1596,6 +1609,55 @@ void init_speed_module(void) #endif } + +/** + * @param µÃµ½Ë®Æ½ÐýתµÄËÙ¶È + * @return none + * +*/ +static volatile float hori_speed_actual = 0; +static volatile float angleDifference = 0; +void as5047d_hori_get_speed(void) +{ + static float lastAngle = 0; + static float lastTime = 0; + float angle = 0; + float time = 0; + + if(g_ptz.hori_start_stop_set == PTZ_HORI_START) { + angle = as5047d_hori_get_angle_a(); + // time = OSTimeGet(); + if (angle < 0 || //·ÀÖ¹¶Á³öµÄ½Ç¶ÈΪ¸ºÖµ + angle > 360.0||//·ÀÖ¹¶Á³öµÄÊýÖµ³¬¹ý360 + isnan(angle) == 1) {//·ÀÖ¹¶Á³öµÄ²»ÊÇÒ»¸öÊý + //ÒÔÉÏ´íÎóµÄÊý¾Ý¶¼¸ÃÉáÆú,´ÓжÁÈ¡ + // goto start; + return; + } + + angleDifference = fabs(angle - lastAngle); + if (angleDifference > 180) { + angleDifference = 360 - angleDifference; + } + + // float timeDifference = time - lastTime; + hori_speed_actual = angleDifference / PTZ_HORI_BIG_GEAR_RATIO * 16.67f; + + lastAngle = angle; + lastTime = time; + } +} + +void as5047d_vert_get_speed(void) +{ + +} + +void as5047d_get_speed_printf(void) +{ + pdebug(DEBUG_LEVEL_INFO, "hori_speed_actual = %f\n", hori_speed_actual); +} + #endif diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index a5d5c79..79fe0ec 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -8,114 +8,114 @@ #ifdef PTZ_LIGHT_GEAR_L6235D_AS5047D #define PID_B 1 - //ˮƽËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_LESS 1.6 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //ˮƽËٶȴóÓÚÉ趨ËÙ¶Èʱ -// #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ -// #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý + //ˮƽ�ٶ�С���趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_LESS 1.6 //ˮƽ�����������ӵı��� + //ˮƽ�ٶȴ����趨�ٶ�ʱ +// #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ�������ٵķ�Χ +// #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ�����������ӵı��� + //��ֱ�ٶ�С���趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //��ֱ�����������ӵı��� #define PTZ_PID_HALL_SPEED 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 3.5//4//1.4 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_HORI_PID_HORI_TI 100.0 //»ý·ÖϵÊý - #define PTZ_HORI_PID_HORI_TD 0.0 //΢·ÖϵÊý + #define PTZ_HORI_PID_HORI_KP 3.5//4//1.4 //0.014//0.005 //����ϵ�� + #define PTZ_HORI_PID_HORI_TI 100.0 //����ϵ�� + #define PTZ_HORI_PID_HORI_TD 0.0 //΢��ϵ�� - #define PTZ_HORI_PID_INPUT_LIMIT 100//6.91//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_HORI_PID_INPUT_LIMIT 100//6.91//PID��������ֵ�޶� + #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� #define PTZ_VERT_PID_T 20u - #define PTZ_VERT_PID_VERT_KP 3//2//1.4 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_VERT_PID_VERT_TI 100.0 //»ý·ÖϵÊý - #define PTZ_VERT_PID_VERT_TD 0.0 //΢·ÖϵÊý + #define PTZ_VERT_PID_VERT_KP 3//2//1.4 //0.014//0.005 //����ϵ�� + #define PTZ_VERT_PID_VERT_TI 100.0 //����ϵ�� + #define PTZ_VERT_PID_VERT_TD 0.0 //΢��ϵ�� - #define PTZ_VERT_PID_INPUT_LIMIT 100//4.0//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_VERT_PID_INPUT_LIMIT 100//4.0//PID��������ֵ�޶� + #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� - #define PTZ_HORI_PID_T_MAX 75//50u//PIDµ÷ËÙ×î´óÖÜÆÚ - #define PTZ_VERT_PID_T_MAX 75//50u//PIDµ÷ËÙ×î´óÖÜÆÚ + #define PTZ_HORI_PID_T_MAX 75//50u//PID����������� + #define PTZ_VERT_PID_T_MAX 75//50u//PID����������� - #define PTZ_HORI_PID_ADD 30//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD 30//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD 30//PID���ٿ������� + #define PTZ_VERT_PID_ADD 30//PID���ٿ������� - #define PTZ_HORI_PID_ADD_A 10//10//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD_A 10//15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD_A 10//10//PID���ٿ������� + #define PTZ_VERT_PID_ADD_A 10//15//PID���ٿ������� #endif #ifdef PTZ_MEDIUM_WORM_L6235D_AS5047D #define PID_B 1 - //ˮƽËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_LESS 1.2 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //ˮƽËٶȴóÓÚÉ趨ËÙ¶Èʱ -// #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ -// #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËٶȴóÓÚÉ趨ËÙ¶Èʱ -// #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ -// #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý + //ˮƽ�ٶ�С���趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_LESS 1.2 //ˮƽ�����������ӵı��� + //ˮƽ�ٶȴ����趨�ٶ�ʱ +// #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ�������ٵķ�Χ +// #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ�����������ӵı��� + //��ֱ�ٶ�С���趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //��ֱ�����������ӵı��� + //��ֱ�ٶȴ����趨�ٶ�ʱ +// #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //��ֱ�������ٵķ�Χ +// #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //��ֱ�����������ӵı��� //#define PID_A 1 - // #define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - // #define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - //ͨ¹ýоƬ·´À¡²âËÙ + // #define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + // #define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + //ͨ��оƬ�������� //#define PTZ_HORI_PID_JY02A_SPEED //#define PTZ_VERT_PID_JY02A_SPEED #define PTZ_PID_HALL_SPEED 1 #define PTZ_HORI_PID_T 20u - #define PTZ_HORI_PID_HORI_KP 0.6//1.0//6.0//±ÈÀýϵÊý - #define PTZ_HORI_PID_HORI_TI 70.0//80.0//100.0//»ý·ÖϵÊý - #define PTZ_HORI_PID_HORI_TD 0.001//΢·ÖϵÊý + #define PTZ_HORI_PID_HORI_KP 0.6//1.0//6.0//����ϵ�� + #define PTZ_HORI_PID_HORI_TI 70.0//80.0//100.0//����ϵ�� + #define PTZ_HORI_PID_HORI_TD 0.001//΢��ϵ�� - #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID��������ֵ�޶� + #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� #define PTZ_VERT_PID_T 20u - #define PTZ_VERT_PID_VERT_KP 6//6.0//±ÈÀýϵÊý - #define PTZ_VERT_PID_VERT_TI 80.0//100.0//»ý·ÖϵÊý - #define PTZ_VERT_PID_VERT_TD 0.001//΢·ÖϵÊý + #define PTZ_VERT_PID_VERT_KP 6//6.0//����ϵ�� + #define PTZ_VERT_PID_VERT_TI 80.0//100.0//����ϵ�� + #define PTZ_VERT_PID_VERT_TD 0.001//΢��ϵ�� - #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID��������ֵ�޶� + #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� - #define PTZ_HORI_PID_T_MAX 80//75u//PIDµ÷ËÙ×î´óÖÜÆÚ - #define PTZ_VERT_PID_T_MAX 80//75u//PIDµ÷ËÙ×î´óÖÜÆÚ + #define PTZ_HORI_PID_T_MAX 80//75u//PID����������� + #define PTZ_VERT_PID_T_MAX 80//75u//PID����������� - #define PTZ_HORI_PID_ADD 20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD 20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD 20//PID���ٿ������� + #define PTZ_VERT_PID_ADD 20//PID���ٿ������� - #define PTZ_HORI_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD_A 15//PID���ٿ������� + #define PTZ_VERT_PID_ADD_A 15//PID���ٿ������� #endif #ifdef PTZ_HEAVY_WORM_L6235D_AS5047D #define PID_B 1 - //ˮƽËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_LESS 1.4 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //ˮƽËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_LESS 1.4 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý + //ˮƽ�ٶ�С���趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_LESS 1.4 //ˮƽ�����������ӵı��� + //ˮƽ�ٶȴ����趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ�����������ӵı��� + //��ֱ�ٶ�С���趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_LESS 1.4 //��ֱ�����������ӵı��� + //��ֱ�ٶȴ����趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //��ֱ�����������ӵı��� @@ -127,149 +127,149 @@ #define PTZ_HORI_PID_T 50u - #define PTZ_HORI_PID_HORI_KP 10.0//3.0 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_HORI_PID_HORI_TI 100.0 //»ý·ÖϵÊý - #define PTZ_HORI_PID_HORI_TD 0.0 //΢·ÖϵÊý + #define PTZ_HORI_PID_HORI_KP 10.0//3.0 //0.014//0.005 //����ϵ�� + #define PTZ_HORI_PID_HORI_TI 100.0 //����ϵ�� + #define PTZ_HORI_PID_HORI_TD 0.0 //΢��ϵ�� - #define PTZ_HORI_PID_INPUT_LIMIT 6.91//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_HORI_PID_INPUT_LIMIT 6.91//PID��������ֵ�޶� + #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� #define PTZ_VERT_PID_T 50u - #define PTZ_VERT_PID_VERT_KP 20.0//3.0 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_VERT_PID_VERT_TI 100.0 //»ý·ÖϵÊý - #define PTZ_VERT_PID_VERT_TD 0.0 //΢·ÖϵÊý + #define PTZ_VERT_PID_VERT_KP 20.0//3.0 //0.014//0.005 //����ϵ�� + #define PTZ_VERT_PID_VERT_TI 100.0 //����ϵ�� + #define PTZ_VERT_PID_VERT_TD 0.0 //΢��ϵ�� - #define PTZ_VERT_PID_INPUT_LIMIT 4.0//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_VERT_PID_INPUT_LIMIT 4.0//PID��������ֵ�޶� + #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� - #define PTZ_HORI_PID_T_MAX 75//50u//PIDµ÷ËÙ×î´óÖÜÆÚ - #define PTZ_VERT_PID_T_MAX 75//50u//PIDµ÷ËÙ×î´óÖÜÆÚ + #define PTZ_HORI_PID_T_MAX 75//50u//PID����������� + #define PTZ_VERT_PID_T_MAX 75//50u//PID����������� - #define PTZ_HORI_PID_ADD 30//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD 30//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD 30//PID���ٿ������� + #define PTZ_VERT_PID_ADD 30//PID���ٿ������� - #define PTZ_HORI_PID_ADD_A 20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD_A 20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD_A 20//PID���ٿ������� + #define PTZ_VERT_PID_ADD_A 20//PID���ٿ������� #endif #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_12V #define PID_B 1 - //ˮƽËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.6 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_LESS 1.3 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //ˮƽËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.6 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_LESS 1.3 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý + //ˮƽ�ٶ�С���趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.6 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_LESS 1.3 //ˮƽ�����������ӵı��� + //ˮƽ�ٶȴ����趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ�����������ӵı��� + //��ֱ�ٶ�С���趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.6 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_LESS 1.3 //��ֱ�����������ӵı��� + //��ֱ�ٶȴ����趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //��ֱ�����������ӵı��� - //#define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - //#define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - //ͨ¹ýоƬ·´À¡²âËÙ + //#define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + //#define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + //ͨ��оƬ�������� //#define PTZ_HORI_PID_JY02A_SPEED //#define PTZ_VERT_PID_JY02A_SPEED #define PTZ_PID_HALL_SPEED 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 2.0//6.0 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_HORI_PID_HORI_TI 100.0 //»ý·ÖϵÊý - #define PTZ_HORI_PID_HORI_TD 0.0 //΢·ÖϵÊý + #define PTZ_HORI_PID_HORI_KP 2.0//6.0 //0.014//0.005 //����ϵ�� + #define PTZ_HORI_PID_HORI_TI 100.0 //����ϵ�� + #define PTZ_HORI_PID_HORI_TD 0.0 //΢��ϵ�� - #define PTZ_HORI_PID_INPUT_LIMIT 100.0//6.91//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_HORI_PID_INPUT_LIMIT 100.0//6.91//PID��������ֵ�޶� + #define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� #define PTZ_VERT_PID_T 30u - #define PTZ_VERT_PID_VERT_KP 2.0//6.0 //0.014//0.005 //±ÈÀýϵÊý - #define PTZ_VERT_PID_VERT_TI 100.0 //»ý·ÖϵÊý - #define PTZ_VERT_PID_VERT_TD 0.0 //΢·ÖϵÊý + #define PTZ_VERT_PID_VERT_KP 2.0//6.0 //0.014//0.005 //����ϵ�� + #define PTZ_VERT_PID_VERT_TI 100.0 //����ϵ�� + #define PTZ_VERT_PID_VERT_TD 0.0 //΢��ϵ�� - #define PTZ_VERT_PID_INPUT_LIMIT 100.0//4.0//PIDµ÷ËÙÊäÈëÖµÏÞ¶¨ - #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_VERT_PID_INPUT_LIMIT 100.0//4.0//PID��������ֵ�޶� + #define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� - #define PTZ_HORI_PID_T_MAX 100u//PIDµ÷ËÙ×î´óÖÜÆÚ - #define PTZ_VERT_PID_T_MAX 100u//PIDµ÷ËÙ×î´óÖÜÆÚ + #define PTZ_HORI_PID_T_MAX 100u//PID����������� + #define PTZ_VERT_PID_T_MAX 100u//PID����������� - #define PTZ_HORI_PID_ADD 20//40//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD 20//40//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD 20//40//PID���ٿ������� + #define PTZ_VERT_PID_ADD 20//40//PID���ٿ������� - #define PTZ_HORI_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD_A 15//PID���ٿ������� + #define PTZ_VERT_PID_ADD_A 15//PID���ٿ������� #endif #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_24V #define PID_B 1 - //ˮƽËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_LESS 1.2 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //ˮƽËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËÙ¶ÈСÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý - //´¹Ö±ËٶȴóÓÚÉ趨ËÙ¶Èʱ - #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //´¹Ö±±ÈÀýµ÷Ëٵķ¶Î§ - #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //´¹Ö±±ÈÀýµ÷ËÙÔö¼ÓµÄ±¶Êý + //ˮƽ�ٶ�С���趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_LESS 0.4 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_LESS 1.2 //ˮƽ�����������ӵı��� + //ˮƽ�ٶȴ����趨�ٶ�ʱ + #define PTZ_HORI_PRO_ADD_RANGE_MORE 0.5 //ˮƽ�������ٵķ�Χ + #define PTZ_HORI_PRO_ADD_MUL_MORE 0.8 //ˮƽ�����������ӵı��� + //��ֱ�ٶ�С���趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_LESS 0.4 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_LESS 1.2 //��ֱ�����������ӵı��� + //��ֱ�ٶȴ����趨�ٶ�ʱ + #define PTZ_VERT_PRO_ADD_RANGE_MORE 0.5 //��ֱ�������ٵķ�Χ + #define PTZ_VERT_PRO_ADD_MUL_MORE 0.8 //��ֱ�����������ӵı��� - //#define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - //#define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//½Ç¶È±ä»¯Òì²½ - //ͨ¹ýоƬ·´À¡²âËÙ + //#define PTZ_HORI_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + //#define PTZ_VERT_PID_AS5048A_ANGLE_ASY_SPEED 2//�Ƕȱ仯�첽 + //ͨ��оƬ�������� //#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_PID_AS5047_SPEED 1 #define PTZ_HORI_PID_T 30u - #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_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 50.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID��������ֵ�޶� + #define PTZ_HORI_PID_OUTPUT_LIMIT 50.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� #define PTZ_VERT_PID_T 30u - #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_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 50.0//PIDµ÷ËÙÊä³öÖµÏÞ¶¨£¬µ±Ç°Êä³öÖµºÍÉÏÒ»´ÎÊä³öÖµÖ®¼äµÄ²îÒì + #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID��������ֵ�޶� + #define PTZ_VERT_PID_OUTPUT_LIMIT 50.0//PID�������ֵ�޶�����ǰ���ֵ����һ�����ֵ֮��IJ��� - #define PTZ_HORI_PID_T_MAX 100u//PIDµ÷ËÙ×î´óÖÜÆÚ - #define PTZ_VERT_PID_T_MAX 100u//PIDµ÷ËÙ×î´óÖÜÆÚ + #define PTZ_HORI_PID_T_MAX 100u//PID����������� + #define PTZ_VERT_PID_T_MAX 100u//PID����������� - #define PTZ_HORI_PID_ADD 20//20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - #define PTZ_VERT_PID_ADD 20//20//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + #define PTZ_HORI_PID_ADD 20//20//PID���ٿ������� + #define PTZ_VERT_PID_ADD 20//20//PID���ٿ������� - //#define PTZ_HORI_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ - //#define PTZ_VERT_PID_ADD_A 15//PIDµ÷ËÙ¿ìËÙÔöÁ¿ + //#define PTZ_HORI_PID_ADD_A 15//PID���ٿ������� + //#define PTZ_VERT_PID_ADD_A 15//PID���ٿ������� #endif -//PIDµ÷ËÙ +//PID���� #define PID_ADJUST_SPEED 0 -//·½Ïòµ÷ËÙ +//������� #define DIRECTOR_ADJUST_SPEED 1 #ifdef PTZ_LIGHT_GEAR_L6235D_AS5047D -//ͨ¹ý·½ÏòPWM²¨Ìṩ·´ÏòÁ¦¾Øµ÷ËÙ -//ÊÇ·ñÆôÓ÷½Ïòµ÷ËÙ +//ͨ������PWM���ṩ�������ص��� +//�Ƿ����÷������ #define PTZ_CONTROL_SPEED_L6235D 2 -//·½Ïòµ÷ËÙ×ÜÖÜÆÚ +//������������� #define PTZ_DIRECTOR_SPEED_T 10//20 #endif @@ -279,6 +279,10 @@ void ptz_pid_Init(); void ptz_hori_pid_clear_zero(); void ptz_vert_pid_clear_zero(); void init_speed_module(void); + +void as5047d_hori_get_speed(void); +void as5047d_vert_get_speed(void); + #endif #endif \ No newline at end of file diff --git a/BSP/Driver/full_bridge/full_bridge.c b/BSP/Driver/full_bridge/full_bridge.c index 671c9b3..1a8a11a 100644 --- a/BSP/Driver/full_bridge/full_bridge.c +++ b/BSP/Driver/full_bridge/full_bridge.c @@ -377,6 +377,7 @@ void h_bldc_six_step() } } + } } /*! diff --git a/GD32F4xx_Firmware_Library/SYSTICK/gd32f4xx_it.c b/GD32F4xx_Firmware_Library/SYSTICK/gd32f4xx_it.c index 994f68c..c0eaed8 100644 --- a/GD32F4xx_Firmware_Library/SYSTICK/gd32f4xx_it.c +++ b/GD32F4xx_Firmware_Library/SYSTICK/gd32f4xx_it.c @@ -391,6 +391,10 @@ void TIMER4_IRQHandler(void) v_bldc_six_step(); h_bldc_six_step(); + + as5047d_hori_get_speed(); + as5047d_vert_get_speed(); + timer_interrupt_flag_clear(TIMER4,TIMER_INT_FLAG_UP); } }