pengsx
/
LW21-01B
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: pengsx:master
Branches Tags
pengsx:master
pengsx:H3k_V10k-speed
pengsx:原始测试程序
pengsx:test
pengsx:H3k_V10k
pengsx:SL_gigabitSwitch_3626motor
..
compare: pengsx:H3k_V10k-speed
Branches Tags
pengsx:H3k_V10k-speed
pengsx:原始测试程序
pengsx:test
pengsx:H3k_V10k
pengsx:master
pengsx:SL_gigabitSwitch_3626motor

6 Commits
master ... H3k_V10k-s

Author SHA1 Message Date
起床就犯困 9d226bec35 测试hall速度计算 2025-06-28 09:00:50 +08:00
起床就犯困 012a3bec8c 定时中断调节速度 2025-06-14 15:08:50 +08:00
起床就犯困 8f6daf1e41 新型电机正常运转 2025-06-12 09:45:45 +08:00
起床就犯困 337edab694 修改过流保护，添加俯仰速度计算（有误） 2025-06-10 17:26:14 +08:00
起床就犯困 845e5d73d5 新电机完成hall+定时器完成水平调速 2025-06-09 09:44:39 +08:00
起床就犯困 9b021d1a0d 修该减速比 2025-05-30 16:36:19 +08:00
13 changed files with 1052 additions and 373 deletions
Show Stats Expand all files Collapse all files

5
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,5 @@
{
"files.associations": {
"speed_to_hall.h": "c"
}
}

48
APP/Appcfg/app.c
View File

@ -50,6 +50,8 @@
#include "systick.h" #include "systick.h"
#include "service_error_count.h" #include "service_error_count.h"
#include "speed_to_hall.h"
void task_printf_init(); void task_printf_init();
// //
@ -111,7 +113,7 @@ static void task_start (void *p_arg)
// ck_f=rcu_clock_freq_get(CK_APB1); // ck_f=rcu_clock_freq_get(CK_APB1);
OSTimeDlyHMSM(0u, 0u, 0u, 100u); OSTimeDlyHMSM(0u, 0u, 0u, 100u);
//蜂鸣器开，初始化开始 //蜂鸣器开，初始化开始
beep_enable(); // beep_enable();
//flash引脚初始化 //flash引脚初始化
Flash_GPIO_Init(); Flash_GPIO_Init();
@ -126,6 +128,7 @@ static void task_start (void *p_arg)
// term_printf("\n udp init \r\n\r\n"); // term_printf("\n udp init \r\n\r\n");
OSTimeDlyHMSM(0u, 0u, 0u, 100u); OSTimeDlyHMSM(0u, 0u, 0u, 100u);
init_hall_speed_module();
//光电开关初始化 //光电开关初始化
init_photoelectric_switch_module(); init_photoelectric_switch_module();
@ -195,7 +198,7 @@ static void task_start (void *p_arg)
init_angle_module(); init_angle_module();
// term_printf("\n angle init \r\n\r\n"); // term_printf("\n angle init \r\n\r\n");
// task_printf_init(); task_printf_init();
//自检模块初始化 //自检模块初始化
init_self_check_module(); init_self_check_module();
@ -227,10 +230,10 @@ int main (void)
CPU_INT08U name_err; CPU_INT08U name_err;
CPU_Init(); CPU_Init();
Mem_Init(); Mem_Init();
Math_Init(); Math_Init();
BSP_IntDisAll(); BSP_IntDisAll();
OSInit(); OSInit();
task_err = OSTaskCreateExt((void (*)(void *)) task_start, task_err = OSTaskCreateExt((void (*)(void *)) task_start,
(void *) 0, (void *) 0,
@ -266,20 +269,29 @@ int main (void)
*/ */
static void task_print() static void task_print()
{ {
OS_TCB *p_tcb; // OS_TCB *p_tcb;
OS_STK_DATA stkDat; // OS_STK_DATA stkDat;
p_tcb = &OSTCBTbl[0]; // p_tcb = &OSTCBTbl[0];
char cpuusage = OSCPUUsage; // char cpuusage = OSCPUUsage;
term_printf(" Prio Used Free Per cpuusage TaskName\r\n"); // term_printf(" Prio Used Free Per cpuusage TaskName\r\n");
while(p_tcb != NULL) // while(p_tcb != NULL)
{ // {
OSTaskStkChk(p_tcb->OSTCBPrio, &stkDat); // OSTaskStkChk(p_tcb->OSTCBPrio, &stkDat);
term_printf(" %2d %5d %5d %02d%% %d %s\r\n ",p_tcb->OSTCBPrio, stkDat.OSUsed, stkDat.OSFree, (stkDat.OSUsed * 100)/(stkDat.OSUsed + stkDat.OSFree), cpuusage, p_tcb->OSTCBTaskName); // term_printf(" %2d %5d %5d %02d%% %d %s\r\n ",p_tcb->OSTCBPrio, stkDat.OSUsed, stkDat.OSFree, (stkDat.OSUsed * 100)/(stkDat.OSUsed + stkDat.OSFree), cpuusage, p_tcb->OSTCBTaskName);
p_tcb = p_tcb->OSTCBPrev; // p_tcb = p_tcb->OSTCBPrev;
} // }
term_printf("\r\n"); // term_printf("\r\n");
// term_printf("h_angle:%f, v_angle:%f\n", as5047d_hori_get_angle_a(), as5047d_vert_get_angle_a());
// term_printf("PidUT_uint : %d \n", g_ptz.hori_pid.PidUT_uint);
// pdebug(DEBUG_LEVEL_DEBUG, "PidUT_uint : %d \n", g_ptz.hori_pid.PidUT_uint);
static char data[50] = {0};
sprintf(data, "PidUT_uint : %d\n", g_ptz.hori_pid.PidUT_uint);
ptz_uart_dev_send(uart_485_handle, data, sizeof(data));
OSTimeDlyHMSM(0u, 0u, 1u, 0u); OSTimeDlyHMSM(0u, 0u, 1u, 0u);
} }

539
APP/Common/ptz_default_value.h
View File

@ -3,507 +3,558 @@
#include "ptz_type_select.h" #include "ptz_type_select.h"
#ifdef PTZ_LIGHT_GEAR_L6235D_AS5047D #ifdef PTZ_LIGHT_GEAR_L6235D_AS5047D
///水平电机减速比 ///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 100.0 #define PTZ_HORI_MOTOR_RATIO 100.0
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 4.05 #define PTZ_HORI_BIG_GEAR_RATIO 4.05
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 405.0 #define PTZ_HORI_RATIO 405.0
///水平电机调速模拟电压最大值 ///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999 #define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值 ///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 0 #define PTZ_HORI_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 400.0//600.0 #define PTZ_HORI_MOTOR_MIN_SPEED 400.0//600.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 7.5 #define PTZ_HORI_MAX_SPEED 7.5
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 1.0//1.5//0.65 #define PTZ_HORI_MIN_SPEED 1.0//1.5//0.65
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 5.0//4.0 #define PTZ_HORI_BEST_SPEED 5.0//4.0
/**********************************************************/ /**********************************************************/
///磁编码器和电机之间的减速比 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 200.0 #define PTZ_VERT_MOTOR_RATIO 200.0
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 7.00 #define PTZ_VERT_BIG_GEAR_RATIO 7.00
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 1400.0 #define PTZ_VERT_RATIO 1400.0
///垂直电机调速模拟电压最大值 ///垂直电机调速模拟电压最大值
#define PTZ_VERT_VR_MAX 1999 #define PTZ_VERT_VR_MAX 1999
///垂直电机调速模拟电压最小值 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 0 #define PTZ_VERT_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 300.0 #define PTZ_VERT_MOTOR_MIN_SPEED 300.0
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 2.20 #define PTZ_VERT_MAX_SPEED 2.20
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.22//0.43 #define PTZ_VERT_MIN_SPEED 0.22//0.43
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 1.7//1.8 #define PTZ_VERT_BEST_SPEED 1.7//1.8
//电机磁极对数 //电机磁极对数
#define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0
//电机磁极对数 //电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0
#endif #endif
#ifdef PTZ_MEDIUM_WORM_L6235D_AS5047D #ifdef PTZ_MEDIUM_WORM_L6235D_AS5047D
///*******************************标准云台******************************/ ///*******************************标准云台******************************/
///水平电机减速比 ///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 25.0//(25.0 * 1.5)//20 #define PTZ_HORI_MOTOR_RATIO 25.0//(25.0 * 1.5)//20
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 80.0//62.0 #define PTZ_HORI_BIG_GEAR_RATIO 80.0//62.0
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 2000.0//2325//1240.0 #define PTZ_HORI_RATIO 2000.0//2325//1240.0
///水平电机调速模拟电压最大值 ///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999 #define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值 ///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 0 #define PTZ_HORI_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 400.0//600.0 #define PTZ_HORI_MOTOR_MIN_SPEED 400.0//600.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 1.5//1.29//2.4 #define PTZ_HORI_MAX_SPEED 1.5//1.29//2.4
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.2//0.3//0.26//0.4 #define PTZ_HORI_MIN_SPEED 0.2//0.3//0.26//0.4
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 1.2//2.0 #define PTZ_HORI_BEST_SPEED 1.2//2.0
/**********************************************************/ /**********************************************************/
///磁编码器和电机之间的减速比 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 75.0//(50.0 * (52.0 / 35.0))//20.0 #define PTZ_VERT_MOTOR_RATIO 75.0//(50.0 * (52.0 / 35.0))//20.0
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 80.0//54.00 #define PTZ_VERT_BIG_GEAR_RATIO 80.0//54.00
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 6000.0//4011.429//1080.0 #define PTZ_VERT_RATIO 6000.0//4011.429//1080.0
///垂直电机调速模拟电压最大值 ///垂直电机调速模拟电压最大值
#define PTZ_VERT_VR_MAX 1999 #define PTZ_VERT_VR_MAX 1999
///垂直电机调速模拟电压最小值 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 0 #define PTZ_VERT_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 400.0//600.0 #define PTZ_VERT_MOTOR_MIN_SPEED 400.0//600.0
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 0.5//0.74//2.7 #define PTZ_VERT_MAX_SPEED 0.5//0.74//2.7
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.07//0.1//0.15//0.5 #define PTZ_VERT_MIN_SPEED 0.07//0.1//0.15//0.5
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 0.5//0.7//2.0 #define PTZ_VERT_BEST_SPEED 0.5//0.7//2.0
//电机磁极对数 //电机磁极对数
#define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0
//电机磁极对数 //电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0
/*******************************激光云台定制******************************/ /*******************************激光云台定制******************************/
// ///水平电机减速比 // ///水平电机减速比
// #define PTZ_HORI_MOTOR_RATIO (100.0 * 1.5) // #define PTZ_HORI_MOTOR_RATIO (100.0 * 1.5)
// ///水平大齿轮减速比 // ///水平大齿轮减速比
// #define PTZ_HORI_BIG_GEAR_RATIO 62.0 // #define PTZ_HORI_BIG_GEAR_RATIO 62.0
// ///水平总减速比 // ///水平总减速比
// #define PTZ_HORI_RATIO 9300 // #define PTZ_HORI_RATIO 9300
// ///水平电机调速模拟电压最大值 // ///水平电机调速模拟电压最大值
// #define PTZ_HORI_VR_MAX 4095 // #define PTZ_HORI_VR_MAX 4095
// ///水平电机调速模拟电压最小值 // ///水平电机调速模拟电压最小值
// #define PTZ_HORI_VR_MIN 0 // #define PTZ_HORI_VR_MIN 0
// //
// ///转速单位 转/每分 // ///转速单位 转/每分
// ///水平电机最大转速 // ///水平电机最大转速
// #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 // #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0
// ///水平电机最小转速 // ///水平电机最小转速
// #define PTZ_HORI_MOTOR_MIN_SPEED 600.0 // #define PTZ_HORI_MOTOR_MIN_SPEED 600.0
// ///水平云台最大转速 // ///水平云台最大转速
// #define PTZ_HORI_MAX_SPEED 0.32 // #define PTZ_HORI_MAX_SPEED 0.32
// ///水平云台最小转速 // ///水平云台最小转速
// #define PTZ_HORI_MIN_SPEED 0.06 // #define PTZ_HORI_MIN_SPEED 0.06
// ///水平云台默认最佳速度 // ///水平云台默认最佳速度
// #define PTZ_HORI_BEST_SPEED 0.32 // #define PTZ_HORI_BEST_SPEED 0.32
// //
// /**********************************************************/ // /**********************************************************/
// //
// ///磁编码器和电机之间的减速比 // ///磁编码器和电机之间的减速比
// #define PTZ_VERT_MOTOR_RATIO (50.0 * (52.0 / 35.0)) // #define PTZ_VERT_MOTOR_RATIO (50.0 * (52.0 / 35.0))
// ///磁编码器与云台轴之间的减速比 // ///磁编码器与云台轴之间的减速比
// #define PTZ_VERT_BIG_GEAR_RATIO 54.00 // #define PTZ_VERT_BIG_GEAR_RATIO 54.00
// ///垂直总减速比 // ///垂直总减速比
// #define PTZ_VERT_RATIO 4011.429 // #define PTZ_VERT_RATIO 4011.429
// ///垂直电机调速模拟电压最大值 // ///垂直电机调速模拟电压最大值
// #define PTZ_VERT_VR_MAX 4095 // #define PTZ_VERT_VR_MAX 4095
// ///垂直电机调速模拟电压最小值 // ///垂直电机调速模拟电压最小值
// #define PTZ_VERT_VR_MIN 0 // #define PTZ_VERT_VR_MIN 0
// //
// ///转速单位 转/每分 // ///转速单位 转/每分
// ///垂直电机最大转速 // ///垂直电机最大转速
// #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 // #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
// ///垂直电机最小转速 // ///垂直电机最小转速
// #define PTZ_VERT_MOTOR_MIN_SPEED 600.0 // #define PTZ_VERT_MOTOR_MIN_SPEED 600.0
// ///垂直云台最大转速 // ///垂直云台最大转速
// #define PTZ_VERT_MAX_SPEED 0.74 // #define PTZ_VERT_MAX_SPEED 0.74
// ///垂直云台最小转速 // ///垂直云台最小转速
// #define PTZ_VERT_MIN_SPEED 0.15 // #define PTZ_VERT_MIN_SPEED 0.15
// ///垂直云台默认最佳速度 // ///垂直云台默认最佳速度
// #define PTZ_VERT_BEST_SPEED 0.7 // #define PTZ_VERT_BEST_SPEED 0.7
// //
// //
// //电机磁极对数 // //电机磁极对数
// #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 // #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0
// //电机霍尔传感器对数 // //电机霍尔传感器对数
// #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 // #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
// //电机旋转一圈FG返回的霍尔脉冲数 // //电机旋转一圈FG返回的霍尔脉冲数
// #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 // #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0
// //
// //电机磁极对数 // //电机磁极对数
// #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 // #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0
// //电机霍尔传感器对数 // //电机霍尔传感器对数
// #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 // #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
// //电机旋转一圈FG返回的霍尔脉冲数 // //电机旋转一圈FG返回的霍尔脉冲数
// #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 // #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0
#endif #endif
#ifdef PTZ_HEAVY_WORM_L6235D_AS5047D #ifdef PTZ_HEAVY_WORM_L6235D_AS5047D
///水平电机减速比************************************标准 ///水平电机减速比************************************标准
#define PTZ_HORI_MOTOR_RATIO 25.0//(25.0 * 1.0) #define PTZ_HORI_MOTOR_RATIO 25.0//(25.0 * 1.0)
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 52.0 #define PTZ_HORI_BIG_GEAR_RATIO 52.0
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 1300.0 #define PTZ_HORI_RATIO 1300.0
///水平电机调速模拟电压最大值 ///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999 #define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值 ///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 0 #define PTZ_HORI_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 300.0 #define PTZ_HORI_MOTOR_MIN_SPEED 300.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 2.3 #define PTZ_HORI_MAX_SPEED 2.3
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.2 #define PTZ_HORI_MIN_SPEED 0.2
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 2.1 #define PTZ_HORI_BEST_SPEED 2.1
///磁编码器和电机之间的减速比************************************标准 ///磁编码器和电机之间的减速比************************************标准
#define PTZ_VERT_MOTOR_RATIO 125.8065//(50.0 * (78.0 / 31.0)) #define PTZ_VERT_MOTOR_RATIO 125.8065//(50.0 * (78.0 / 31.0))
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 60.0 #define PTZ_VERT_BIG_GEAR_RATIO 60.0
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 7548.3871 #define PTZ_VERT_RATIO 7548.3871
///垂直电机调速模拟电压最大值 ///垂直电机调速模拟电压最大值
#define PTZ_VERT_VR_MAX 1999 #define PTZ_VERT_VR_MAX 1999
///垂直电机调速模拟电压最小值 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 0 #define PTZ_VERT_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 600.0 #define PTZ_VERT_MOTOR_MIN_SPEED 600.0
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 0.4 #define PTZ_VERT_MAX_SPEED 0.4
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.08 #define PTZ_VERT_MIN_SPEED 0.08
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 0.30 #define PTZ_VERT_BEST_SPEED 0.30
//电机磁极对数 //电机磁极对数
#define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0
//电机磁极对数 //电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0
#endif #endif
#ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_12V #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_12V
/*****************************第二个版本**********************************/ /*****************************第二个版本**********************************/
///水平电机减速比 ///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 27.0 #define PTZ_HORI_MOTOR_RATIO 27.0
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 50.0 #define PTZ_HORI_BIG_GEAR_RATIO 50.0
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 1350.0 #define PTZ_HORI_RATIO 1350.0
///水平电机调速模拟电压最大值 ///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999 #define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值 ///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 0 #define PTZ_HORI_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 2000//2000.0 #define PTZ_HORI_MOTOR_MAX_SPEED 2000//2000.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 405.0 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 1.48//1.48 #define PTZ_HORI_MAX_SPEED 1.48//1.48
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.3 #define PTZ_HORI_MIN_SPEED 0.3
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 1.2 #define PTZ_HORI_BEST_SPEED 1.2
/**********************************************************/ /**********************************************************/
///磁编码器和电机之间的减速比 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 27.0//19.2//27.0 #define PTZ_VERT_MOTOR_RATIO 27.0//19.2//27.0
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 50.0 #define PTZ_VERT_BIG_GEAR_RATIO 50.0
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 1350.0//960.0//1350.0 #define PTZ_VERT_RATIO 1350.0//960.0//1350.0
///垂直电机调速模拟电压最大值 ///垂直电机调速模拟电压最大值
#define PTZ_VERT_VR_MAX 1999 #define PTZ_VERT_VR_MAX 1999
///垂直电机调速模拟电压最小值 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 0 #define PTZ_VERT_VR_MIN 0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 2000.0 #define PTZ_VERT_MOTOR_MAX_SPEED 2000.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 405.0 #define PTZ_VERT_MOTOR_MIN_SPEED 405.0
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 1.48//2.08//1.48 #define PTZ_VERT_MAX_SPEED 1.48//2.08//1.48
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.3//0.42//0.3 #define PTZ_VERT_MIN_SPEED 0.3//0.42//0.3
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 1.2//1.8//1.2 #define PTZ_VERT_BEST_SPEED 1.2//1.8//1.2
//电机磁极对数 //电机磁极对数
#define PTZ_HORI_MOTOR_POLE_PAIRS 4.0 #define PTZ_HORI_MOTOR_POLE_PAIRS 4.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 12.0 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 12.0
//电机磁极对数 //电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 4.0 #define PTZ_VERT_MOTOR_POLE_PAIRS 4.0
//电机霍尔传感器对数 //电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0
#endif #endif
#define NEW_MOTOR
#ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_24V #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_24V
#ifndef NEW_MOTOR
///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 27.0
///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 50.0
///水平总减速比
#define PTZ_HORI_RATIO 1350.0
///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 50
///水平电机减速比 ///转速单位 转/每分
#define PTZ_HORI_MOTOR_RATIO 27.0 ///水平电机最大转速
///水平大齿轮减速比 #define PTZ_HORI_MOTOR_MAX_SPEED 3000
#define PTZ_HORI_BIG_GEAR_RATIO 50.0 ///水平电机最小转速
///水平总减速比 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0
#define PTZ_HORI_RATIO 1350.0 ///水平云台最大转速
///水平电机调速模拟电压最大值 #define PTZ_HORI_MAX_SPEED 2.2
#define PTZ_HORI_VR_MAX 1999 ///水平云台最小转速
///水平电机调速模拟电压最小值 #define PTZ_HORI_MIN_SPEED 0.3
#define PTZ_HORI_VR_MIN 0 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 1.5
#endif
#ifdef NEW_MOTOR
///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 3.0
///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 50.0
///水平总减速比
#define PTZ_HORI_RATIO 150.0
///水平电机调速模拟电压最大值
#define PTZ_HORI_VR_MAX 1999
///水平电机调速模拟电压最小值
#define PTZ_HORI_VR_MIN 50
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 3000 #define PTZ_HORI_MOTOR_MAX_SPEED 3000
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 405.0 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 2.2 #define PTZ_HORI_MAX_SPEED 20.0
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.3 #define PTZ_HORI_MIN_SPEED 2.7
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 1.6 #define PTZ_HORI_BEST_SPEED 14.4
#endif
/**********************************************************/ /**********************************************************/
#ifndef NEW_MOTOR
///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 27.0
///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 50.0
///垂直总减速比
#define PTZ_VERT_RATIO 1350.0
///垂直电机调速模拟电压最大值
#define PTZ_VERT_VR_MAX 1899
///垂直电机调速模拟电压最小值
#define PTZ_VERT_VR_MIN 50
///磁编码器和电机之间的减速比 ///转速单位 转/每分
#define PTZ_VERT_MOTOR_RATIO 27.0 ///垂直电机最大转速
///磁编码器与云台轴之间的减速比 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
#define PTZ_VERT_BIG_GEAR_RATIO 50.0 ///垂直电机最小转速
///垂直总减速比 #define PTZ_VERT_MOTOR_MIN_SPEED 405.0
#define PTZ_VERT_RATIO 1350.0 ///垂直云台最大转速
///垂直电机调速模拟电压最大值 #define PTZ_VERT_MAX_SPEED 2.2
#define PTZ_VERT_VR_MAX 1999 ///垂直云台最小转速
///垂直电机调速模拟电压最小值 #define PTZ_VERT_MIN_SPEED 0.3
#define PTZ_VERT_VR_MIN 0 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 1.5
#endif
///转速单位 转/每分 #ifdef NEW_MOTOR
///垂直电机最大转速 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 #define PTZ_VERT_MOTOR_RATIO 10.0
///垂直电机最小转速 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_MOTOR_MIN_SPEED 405.0 #define PTZ_VERT_BIG_GEAR_RATIO 50.0
///垂直云台最大转速 ///垂直总减速比
#define PTZ_VERT_MAX_SPEED 2.2 #define PTZ_VERT_RATIO 500.0
///垂直云台最小转速 ///垂直电机调速模拟电压最大值
#define PTZ_VERT_MIN_SPEED 0.3 #define PTZ_VERT_VR_MAX 1899
///垂直云台默认最佳速度 ///垂直电机调速模拟电压最小值
#define PTZ_VERT_BEST_SPEED 1.6 #define PTZ_VERT_VR_MIN 100
///转速单位 转/每分
///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 3000.0
///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 405.0
///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 6.0
///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.81
///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 4.3
//电机磁极对数 #endif
#define PTZ_HORI_MOTOR_POLE_PAIRS 4.0 //电机磁极对数
//电机霍尔传感器对数 #define PTZ_HORI_MOTOR_POLE_PAIRS 4.0
#define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 //电机霍尔传感器对数
//电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 12.0 //电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_HORI_ONE_CYCLE_HALL_NUM 12.0
//电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 4.0
//电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0
//电机磁极对数
#define PTZ_VERT_MOTOR_POLE_PAIRS 4.0
//电机霍尔传感器对数
#define PTZ_VERT_MOTOR_HALL_PAIRS 3.0
//电机旋转一圈FG返回的霍尔脉冲数
#define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0
#endif #endif
//步进电机 //步进电机
#ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V #ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V
///水平电机减速比 ///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 1.0 #define PTZ_HORI_MOTOR_RATIO 1.0
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 50.0 #define PTZ_HORI_BIG_GEAR_RATIO 50.0
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 50.0 #define PTZ_HORI_RATIO 50.0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 750.0 #define PTZ_HORI_MOTOR_MAX_SPEED 750.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 1.0 #define PTZ_HORI_MOTOR_MIN_SPEED 1.0
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 15.0 #define PTZ_HORI_MAX_SPEED 15.0
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.02 #define PTZ_HORI_MIN_SPEED 0.02
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 2.0//10.0 //步进电机默认速度不能过大，应为对应扭矩最大值 #define PTZ_HORI_BEST_SPEED 2.0//10.0 //步进电机默认速度不能过大，应为对应扭矩最大值
///快要到达指定位置时的刹车转速 ///快要到达指定位置时的刹车转速
#define PTZ_HORI_BREAK_SPEED 0.3 #define PTZ_HORI_BREAK_SPEED 0.3
/**********************************************************/ /**********************************************************/
///磁编码器和电机之间的减速比 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 1.0 #define PTZ_VERT_MOTOR_RATIO 1.0
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 50.0 #define PTZ_VERT_BIG_GEAR_RATIO 50.0
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 50.0 #define PTZ_VERT_RATIO 50.0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 500//300.0 #define PTZ_VERT_MOTOR_MAX_SPEED 500//300.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 1.0 #define PTZ_VERT_MOTOR_MIN_SPEED 1.0
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 10.0 #define PTZ_VERT_MAX_SPEED 10.0
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.02 #define PTZ_VERT_MIN_SPEED 0.02
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 2.0//6.0 #define PTZ_VERT_BEST_SPEED 2.0//6.0
///快要到达指定位置时的刹车转速 ///快要到达指定位置时的刹车转速
#define PTZ_VERT_BREAK_SPEED 0.3 #define PTZ_VERT_BREAK_SPEED 0.3
///////////////////**////////////////////////////// ///////////////////**//////////////////////////////
///步进电机默认步距角 ///步进电机默认步距角
#define PTZ_HORI_MOTOR_STEP 1.8 #define PTZ_HORI_MOTOR_STEP 1.8
///步进电机默认步距角 ///步进电机默认步距角
#define PTZ_VERT_MOTOR_STEP 1.8 #define PTZ_VERT_MOTOR_STEP 1.8
#endif #endif
#ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V #ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V
///水平电机减速比 ///水平电机减速比
#define PTZ_HORI_MOTOR_RATIO 1.0 #define PTZ_HORI_MOTOR_RATIO 1.0
///水平大齿轮减速比 ///水平大齿轮减速比
#define PTZ_HORI_BIG_GEAR_RATIO 80.0 #define PTZ_HORI_BIG_GEAR_RATIO 80.0
///水平总减速比 ///水平总减速比
#define PTZ_HORI_RATIO 80.0 #define PTZ_HORI_RATIO 80.0
///转速单位 转/每分 ///转速单位 转/每分
///水平电机最大转速 ///水平电机最大转速
#define PTZ_HORI_MOTOR_MAX_SPEED 500.0 #define PTZ_HORI_MOTOR_MAX_SPEED 500.0
///水平电机最小转速 ///水平电机最小转速
#define PTZ_HORI_MOTOR_MIN_SPEED 1.6 #define PTZ_HORI_MOTOR_MIN_SPEED 1.6
///水平云台最大转速 ///水平云台最大转速
#define PTZ_HORI_MAX_SPEED 6.25 #define PTZ_HORI_MAX_SPEED 6.25
///水平云台最小转速 ///水平云台最小转速
#define PTZ_HORI_MIN_SPEED 0.02 #define PTZ_HORI_MIN_SPEED 0.02
///水平云台默认最佳速度 ///水平云台默认最佳速度
#define PTZ_HORI_BEST_SPEED 1.5 #define PTZ_HORI_BEST_SPEED 1.5
///快要到达指定位置时的刹车转速 ///快要到达指定位置时的刹车转速
#define PTZ_HORI_BREAK_SPEED 0.3 #define PTZ_HORI_BREAK_SPEED 0.3
/**********************************************************/ /**********************************************************/
///磁编码器和电机之间的减速比 ///磁编码器和电机之间的减速比
#define PTZ_VERT_MOTOR_RATIO 1.5 #define PTZ_VERT_MOTOR_RATIO 1.5
///磁编码器与云台轴之间的减速比 ///磁编码器与云台轴之间的减速比
#define PTZ_VERT_BIG_GEAR_RATIO 80.0 #define PTZ_VERT_BIG_GEAR_RATIO 80.0
///垂直总减速比 ///垂直总减速比
#define PTZ_VERT_RATIO 120.0 #define PTZ_VERT_RATIO 120.0
///转速单位 转/每分 ///转速单位 转/每分
///垂直电机最大转速 ///垂直电机最大转速
#define PTZ_VERT_MOTOR_MAX_SPEED 300.0 #define PTZ_VERT_MOTOR_MAX_SPEED 300.0
///垂直电机最小转速 ///垂直电机最小转速
#define PTZ_VERT_MOTOR_MIN_SPEED 2.4 #define PTZ_VERT_MOTOR_MIN_SPEED 2.4
///垂直云台最大转速 ///垂直云台最大转速
#define PTZ_VERT_MAX_SPEED 2.5 #define PTZ_VERT_MAX_SPEED 2.5
///垂直云台最小转速 ///垂直云台最小转速
#define PTZ_VERT_MIN_SPEED 0.02 #define PTZ_VERT_MIN_SPEED 0.02
///垂直云台默认最佳速度 ///垂直云台默认最佳速度
#define PTZ_VERT_BEST_SPEED 0.8 #define PTZ_VERT_BEST_SPEED 0.8
///快要到达指定位置时的刹车转速 ///快要到达指定位置时的刹车转速
#define PTZ_VERT_BREAK_SPEED 0.3 #define PTZ_VERT_BREAK_SPEED 0.3
///////////////////**////////////////////////////// ///////////////////**//////////////////////////////
///步进电机默认步距角 ///步进电机默认步距角
#define PTZ_HORI_MOTOR_STEP 1.8 #define PTZ_HORI_MOTOR_STEP 1.8
///步进电机默认步距角 ///步进电机默认步距角
#define PTZ_VERT_MOTOR_STEP 1.8 #define PTZ_VERT_MOTOR_STEP 1.8
#endif #endif

2
APP/Device/Device_rotate/rotate_bldc.c
View File

@ -827,7 +827,7 @@ static char ptz_vert_rotate_monitor_task()
case PTZ_VERT_DEC_BRAKE_B: case PTZ_VERT_DEC_BRAKE_B:
k ++;//K增加1,时间增加1ms k ++;//K增加1,时间增加1ms
if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 2 || if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 2 ||
k >= 500) k >= 100)
{ {
k = 0; k = 0;
g_ptz.vert_rotate_monitor_switch = 0; g_ptz.vert_rotate_monitor_switch = 0;

8
APP/Device/Device_rotate/rotate_bldc.h
View File

@ -224,16 +224,16 @@
#define PTZ_VERT_DIR_STOP 2 #define PTZ_VERT_DIR_STOP 2
///距离最近刹车点以最小转速运行的距离 ///距离最近刹车点以最小转速运行的距离
#define PTZ_HORI_MIN_SPEED_ANGLE 5.0 #define PTZ_HORI_MIN_SPEED_ANGLE 6.0
#define PTZ_VERT_MIN_SPEED_ANGLE 5.0 #define PTZ_VERT_MIN_SPEED_ANGLE 6.0
///刹车最近距离 ///刹车最近距离
#define PTZ_HORI_STOP_NEAR_DISTANCE 0.01 #define PTZ_HORI_STOP_NEAR_DISTANCE 0.1
///刹车最远距离 ///刹车最远距离
#define PTZ_HORI_STOP_FAR_DISTANCE 4.0 #define PTZ_HORI_STOP_FAR_DISTANCE 4.0
///刹车最近距离 ///刹车最近距离
#define PTZ_VERT_STOP_NEAR_DISTANCE 0.01 #define PTZ_VERT_STOP_NEAR_DISTANCE 0.1
///刹车最远距离 ///刹车最远距离
#define PTZ_VERT_STOP_FAR_DISTANCE 4.0 #define PTZ_VERT_STOP_FAR_DISTANCE 4.0

478
APP/Device/Device_speed/speed_to_bldc.c
View File

@ -7,8 +7,11 @@
#include "full_bridge.h" #include "full_bridge.h"
#include "as5047d.h" #include "as5047d.h"
#include "speed_to_hall.h"
#ifdef PTZ_BLDC_MOTOR #ifdef PTZ_BLDC_MOTOR
//发送云台实际转速 //发送云台实际转速
void ptz_send_speed(char dev, char speed) void ptz_send_speed(char dev, char speed)
{ {
@ -81,6 +84,14 @@ void ptz_pid_init()
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.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.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.C = g_ptz.vert_pid.KP * (g_ptz.vert_pid.TD / g_ptz.vert_pid.T);
g_speed_to_hall.hori_speed_pid.kp = PTZ_HORI_PID_HORI_KP;
// g_speed_to_hall.hori_speed_pid.ki = PTZ_HORI_PID_HORI_KP * (20.0 / PTZ_HORI_PID_HORI_TI);
g_speed_to_hall.hori_speed_pid.ki = PTZ_HORI_PID_HORI_TI;
g_speed_to_hall.vert_speed_pid.kp = PTZ_VERT_PID_VERT_KP;
// g_speed_to_hall.vert_speed_pid.ki = PTZ_VERT_PID_VERT_KP * (20.0 / PTZ_VERT_PID_VERT_TI);
g_speed_to_hall.vert_speed_pid.ki = PTZ_VERT_PID_VERT_TI;
} }
void ptz_hori_pid_clear_zero() void ptz_hori_pid_clear_zero()
@ -108,7 +119,24 @@ static float ptz_hori_pid_calculate(float H_SampSpeed)
// H_IError = PTZ_HORI_PID_INPUT_LIMIT * -1; // 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_speed_to_hall.hori_speed_pid.kp * (H_IError - g_ptz.hori_pid.LastError)
// + g_speed_to_hall.hori_speed_pid.ki * H_IError;
// H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * (H_IError - g_ptz.hori_pid.LastError);
g_speed_to_hall.hori_speed_pid.sum_error += H_IError;
if (g_speed_to_hall.hori_speed_pid.sum_error > 20000) {
g_speed_to_hall.hori_speed_pid.sum_error = 20000;
}
else if (g_speed_to_hall.hori_speed_pid.sum_error < -20000) {
g_speed_to_hall.hori_speed_pid.sum_error = -20000;
}
H_IIncPid = g_speed_to_hall.hori_speed_pid.kp * H_IError + g_speed_to_hall.hori_speed_pid.ki * g_speed_to_hall.hori_speed_pid.sum_error;
// H_IIncPid = g_speed_to_hall.hori_speed_pid.ki * g_speed_to_hall.hori_speed_pid.sum_error;
// H_IIncPid = PTZ_HORI_PID_HORI_KP;
//存储误差，用于下次计算 //存储误差，用于下次计算
g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError; g_ptz.hori_pid.PrevError = g_ptz.hori_pid.LastError;
g_ptz.hori_pid.LastError = H_IError; g_ptz.hori_pid.LastError = H_IError;
@ -123,6 +151,11 @@ static void ptz_hori_pid_task()
{ {
unsigned int time = 0; unsigned int time = 0;
char i = 0; char i = 0;
uint32_t startTime;
uint32_t endTime;
while(1) while(1)
{ {
if(g_ptz.hori_start_stop_set == PTZ_HORI_START) if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
@ -298,7 +331,8 @@ static void ptz_hori_pid_task()
#endif #endif
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 // #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
#if 0
g_ptz.hori_pid.hall_h1_count = 0; g_ptz.hori_pid.hall_h1_count = 0;
g_ptz.hori_pid.hall_h2_count = 0; g_ptz.hori_pid.hall_h2_count = 0;
g_ptz.hori_pid.hall_h3_count = 0; g_ptz.hori_pid.hall_h3_count = 0;
@ -610,6 +644,95 @@ static void ptz_hori_pid_task()
// } // }
// else // else
#ifdef PTZ_HALL_SPEED_SL
// if (g_ptz.hori_speed_actual == 0) {
// h_pwm_duty_change(PTZ_HORI_VR_MIN);
// }
// /* 使能计算速度 */
// g_speed_to_hall.hori_speed_t.flag = calculation_enable;
// /* 超时机制 */
// uint32_t timeOut = 0;
// /* 电机转动后，等待进入hall中断启动速度计算 */
// while (!(g_speed_to_hall.hori_speed_t.flag == calculation_start
// || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
// && timeOut < 15) {
// OSTimeDlyHMSM(0u, 0u, 0u, 1u);
// timeOut++;
// }
// if (timeOut >= 15) {
// g_ptz.hori_speed_actual = 0;
// goto timeOutErr;
// }
// /* 延时，多个hall信号来计算速度 */
// OSTimeDlyHMSM(0u, 0u, 0u, 50u);
// g_speed_to_hall.hori_speed_t.flag = calculation_ok;
// timeOut = 0;
// /* 等待最后一个hall信号到来 */
// while (!(g_speed_to_hall.hori_speed_t.flag == calculation_end
// || g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
// && timeOut < 10) {
// OSTimeDlyHMSM(0u, 0u, 0u, 1u);
// timeOut++;
// }
// if (timeOut >= 10) {
// g_ptz.hori_speed_actual = 0;
// goto timeOutErr;
// }
// uint32_t time;
// if (g_speed_to_hall.hori_speed_t.endTime_60ms > g_speed_to_hall.hori_speed_t.startTime_60ms) {
// time = (g_speed_to_hall.hori_speed_t.endTime_60ms - g_speed_to_hall.hori_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD
// + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us);
// }
// else if (g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.hori_speed_t.startTime_60ms) {
// time = g_speed_to_hall.hori_speed_t.endTime_us - g_speed_to_hall.hori_speed_t.startTime_us;
// }
// else {
// time = (g_speed_to_hall.hori_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.hori_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD
// + g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us);
// }
// g_ptz.hori_speed_actual = (float)g_speed_to_hall.hori_speed_t.hallNum * 5000000.0 / (float)time / PTZ_HORI_RATIO;
// timeOutErr:
// ;
#if 0
OSTimeDlyHMSM(0u, 0u, 0u, 20u);
uint32_t timeLen;
endTime = OSTimeGet();
timeLen = endTime - startTime;
if (timeLen < 0) {
timeLen = endTime + 0xFFFFFFFFu - startTime;
}
static uint32_t hallNum;
static uint32_t hallLastNum;
hallNum = g_ptz.hori_pid.hall_h1_count + g_ptz.hori_pid.hall_h2_count + g_ptz.hori_pid.hall_h3_count;
if (hallLastNum != hallNum) {
g_ptz.hori_speed_actual = g_speed_to_hall.hori_speed_t.speed / PTZ_HORI_RATIO;
}
else {
g_ptz.hori_speed_actual = 0;
}
hallLastNum = hallNum;
startTime = endTime;
#endif
#endif
#if 0
if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2) if(g_ptz.hori_speed_actual > g_ptz.hori_speed_set * 2)
{//速度大于设定速度的3倍 {//速度大于设定速度的3倍
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * 0.7;////新增直线减速 g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * 0.7;////新增直线减速
@ -620,7 +743,7 @@ static void ptz_hori_pid_task()
else else
{ {
//计算PID控制器输出值 //计算PID控制器输出值
g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float; g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual, timeLen) + g_ptz.hori_pid.LastUT_float;
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大 //即防止PID增量过大
@ -661,7 +784,8 @@ static void ptz_hori_pid_task()
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片 //将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
// hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint); // hori_dac0_data_out(g_ptz.hori_pid.PidUT_uint);
h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint); // h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint);
h_pwm_duty_change(600);
//将当前PID输出值保存 //将当前PID输出值保存
g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float;
@ -669,10 +793,18 @@ static void ptz_hori_pid_task()
if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
{ {
g_speed_to_hall.hori_speed_t.startTime_60ms = TIMER_CNT(TIMER6);
g_speed_to_hall.hori_speed_t.startTime_us = g_speed_to_hall.time_60ms;
g_speed_to_hall.hori_speed_t.speed = 0;
startTime = OSTimeGet();
g_ptz.hori_speed_actual = 0; g_ptz.hori_speed_actual = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 10u); OSTimeDlyHMSM(0u, 0u, 0u, 10u);
} }
#endif
}
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
//防止数据超限溢出 //防止数据超限溢出
if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE) if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE)
@ -689,6 +821,7 @@ static void ptz_hori_pid_task()
} }
#endif #endif
OSTimeDlyHMSM(0u, 0u, 0u, 50u);
} }
} }
@ -754,7 +887,22 @@ static float ptz_vert_pid_calculate(float SampSpeed)
// IError = PTZ_VERT_PID_INPUT_LIMIT * -1; // 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;
g_speed_to_hall.vert_speed_pid.sum_error += IError;
if (g_speed_to_hall.vert_speed_pid.sum_error > 20000) {
g_speed_to_hall.vert_speed_pid.sum_error = 20000;
}
else if (g_speed_to_hall.vert_speed_pid.sum_error < -20000) {
g_speed_to_hall.vert_speed_pid.sum_error = -20000;
}
IIncPid = g_speed_to_hall.vert_speed_pid.kp * IError + g_speed_to_hall.vert_speed_pid.ki * g_speed_to_hall.vert_speed_pid.sum_error;
// IIncPid = PTZ_VERT_PID_VERT_KP * (IError - g_ptz.hori_pid.LastError) + PTZ_VERT_PID_VERT_TI * IError * H_SampTime;
// IIncPid = g_speed_to_hall.vert_speed_pid.kp * (IError - g_ptz.hori_pid.LastError) + g_speed_to_hall.vert_speed_pid.ki * IError;
//存储误差，用于下次计算 //存储误差，用于下次计算
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
g_ptz.vert_pid.LastError = IError; g_ptz.vert_pid.LastError = IError;
@ -771,9 +919,9 @@ static void ptz_vert_pid_task()
char i = 0; char i = 0;
while(1) while(1)
{ {
//计算转速 // //计算转速
if(g_ptz.vert_start_stop_set == PTZ_VERT_START) // if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{ // {
#ifdef PTZ_VERT_PID_as5047d_ANGLE_ASY_SPEED #ifdef PTZ_VERT_PID_as5047d_ANGLE_ASY_SPEED
//磁编码器测速，数据清0 //磁编码器测速，数据清0
g_ptz.vert_as5047d.as5047d_speed_angle_a = 0; g_ptz.vert_as5047d.as5047d_speed_angle_a = 0;
@ -939,8 +1087,9 @@ static void ptz_vert_pid_task()
g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual; g_ptz.vert_speed_actual = g_ptz.vert_speed_jy02a_actual;
#endif #endif
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 // #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
#if 0
g_ptz.vert_pid.hall_h1_count = 0; g_ptz.vert_pid.hall_h1_count = 0;
g_ptz.vert_pid.hall_h2_count = 0; g_ptz.vert_pid.hall_h2_count = 0;
@ -1219,13 +1368,72 @@ static void ptz_vert_pid_task()
#endif #endif
#ifdef PTZ_HALL_SPEED_SL
} // if (g_ptz.vert_speed_actual == 0) {
//PID调速 // v_pwm_duty_change(PTZ_VERT_VR_MIN);
if(g_ptz.vert_start_stop_set == PTZ_VERT_START && // }
g_ptz.vert_pid.mode == PID_ADJUST_SPEED)
{ /* 使能计算速度 */
g_speed_to_hall.vert_speed_t.flag = calculation_enable;
/* 超时机制 */
uint32_t timeOut = 0;
/* 电机转动后，等待进入hall中断启动速度计算 */
while (!(g_speed_to_hall.vert_speed_t.flag == calculation_start
|| g_ptz.vert_start_stop_set == PTZ_HORI_STOP)
&& timeOut < 15) {
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
timeOut++;
}
if (timeOut >= 15) {
g_ptz.vert_speed_actual = 0;
goto timeOutErr;
}
/* 延时，多个hall信号来计算速度 */
OSTimeDlyHMSM(0u, 0u, 0u, 50u);
g_speed_to_hall.vert_speed_t.flag = calculation_ok;
timeOut = 0;
/* 等待最后一个hall信号到来 */
while (!(g_speed_to_hall.vert_speed_t.flag == calculation_end
|| g_ptz.vert_start_stop_set == PTZ_HORI_STOP)
&& timeOut < 10) {
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
timeOut++;
}
if (timeOut >= 10) {
g_ptz.vert_speed_actual = 0;
goto timeOutErr;
}
uint32_t time;
if (g_speed_to_hall.vert_speed_t.endTime_60ms > g_speed_to_hall.vert_speed_t.startTime_60ms) {
time = (g_speed_to_hall.vert_speed_t.endTime_60ms - g_speed_to_hall.vert_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us);
}
else if (g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.vert_speed_t.startTime_60ms) {
time = g_speed_to_hall.vert_speed_t.endTime_us - g_speed_to_hall.vert_speed_t.startTime_us;
}
else {
time = (g_speed_to_hall.vert_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.vert_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us);
}
g_ptz.vert_speed_actual = (float)g_speed_to_hall.vert_speed_t.hallNum * 5000000.0 / (float)time / PTZ_VERT_RATIO;
timeOutErr:
#endif
// }
// //PID调速
// if(g_ptz.vert_start_stop_set == PTZ_VERT_START &&
// g_ptz.vert_pid.mode == PID_ADJUST_SPEED)
// {
#ifdef PTZ_CONTROL_SPEED_L6235D #ifdef PTZ_CONTROL_SPEED_L6235D
//判断是否需要启动方向调速 //判断是否需要启动方向调速
@ -1265,63 +1473,63 @@ static void ptz_vert_pid_task()
// g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; // g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
// } // }
// else // else
if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2) // if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2)
{//速度大于设定速度的3倍 // {//速度大于设定速度的3倍
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float - 100.0;//* 1.3/*PTZ_HORI_PID_ADD*/; // g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float - 100.0;//* 1.3/*PTZ_HORI_PID_ADD*/;
g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError; // g_ptz.vert_pid.PrevError = g_ptz.vert_pid.LastError;
g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; // g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual;
} // }
else // else
{ // {
//计算PID控制器输出值 // //计算PID控制器输出值
g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float; // g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual, time) + g_ptz.vert_pid.LastUT_float;
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内 // //控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大 // //即防止PID增量过大
if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//限定PID输出限定 // if(fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT)//限定PID输出限定
{ // {
if(g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float) // if(g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float)
{ // {
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT; // g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT;
} // }
else // else
{ // {
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT; // g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT;
} // }
} // }
} // }
if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_VR_MIN)//限制输入模拟电压最小值 // if(g_ptz.vert_pid.PidUT_float < PTZ_VERT_VR_MIN)//限制输入模拟电压最小值
{ // {
g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN; // g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MIN;
} // }
if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值 // if(g_ptz.vert_pid.PidUT_float > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值
{ // {
g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX; // g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX;
} // }
g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5); // g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5);
//限制PID的输出值在某个指定的范围 // //限制PID的输出值在某个指定的范围
if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值 // if(g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN)//限制输入模拟电压最小值
{ // {
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN; // g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN;
} // }
if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值 // if(g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX)//限制输入模拟电压最大值
{ // {
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX; // g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX;
} // }
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片 // //将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
// vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint); // // 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);
//将当前PID输出值保存 // //将当前PID输出值保存
g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float; // g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float;
} // }
@ -1385,12 +1593,12 @@ static void ptz_vert_pid_task()
} }
#endif #endif
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) // if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
{ // {
g_ptz.vert_pid.roll_start = 0; // g_ptz.vert_pid.roll_start = 0;
g_ptz.vert_speed_actual = 0; // g_ptz.vert_speed_actual = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 10u); // OSTimeDlyHMSM(0u, 0u, 0u, 10u);
} // }
#ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速 #ifdef PTZ_PID_HALL_SPEED //霍尔反馈测速
//防止数据超限溢出 //防止数据超限溢出
@ -1408,6 +1616,7 @@ static void ptz_vert_pid_task()
} }
#endif #endif
OSTimeDlyHMSM(0u, 0u, 0u, 50u);
} }
} }
@ -1527,6 +1736,129 @@ void init_speed_module(void)
void get_hori_speed(void)
{
if(g_ptz.hori_start_stop_set == PTZ_HORI_START) {
/* hall数量未变，表示速度没有变化 */
static uint32_t hallNum;
static uint32_t hallLastNum;
hallNum = g_ptz.hori_pid.hall_h1_count + g_ptz.hori_pid.hall_h2_count + g_ptz.hori_pid.hall_h3_count;
if (hallLastNum != hallNum) {
g_ptz.hori_speed_actual = g_speed_to_hall.hori_speed_t.speed / PTZ_HORI_RATIO;
}
else {
g_ptz.hori_speed_actual = 0;
}
hallLastNum = hallNum;
//计算PID控制器输出值
// g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual) + g_ptz.hori_pid.LastUT_float;
g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual);
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大
//限定PID输出限定
if (fabs(g_ptz.hori_pid.PidUT_float - g_ptz.hori_pid.LastUT_float) > PTZ_HORI_PID_OUTPUT_LIMIT) {
if (g_ptz.hori_pid.PidUT_float > g_ptz.hori_pid.LastUT_float) {
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float + PTZ_HORI_PID_OUTPUT_LIMIT;
}
else {
g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.LastUT_float - PTZ_HORI_PID_OUTPUT_LIMIT;
}
}
g_ptz.hori_pid.PidUT_uint = (unsigned int)(g_ptz.hori_pid.PidUT_float + 0.5);
//限制PID的输出值在某个指定的范围
//限制输入模拟电压最小值
if (g_ptz.hori_pid.PidUT_uint <= PTZ_HORI_VR_MIN) {
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MIN;
}
//限制输入模拟电压最大值
if (g_ptz.hori_pid.PidUT_uint > PTZ_HORI_VR_MAX) {
g_ptz.hori_pid.PidUT_uint = PTZ_HORI_VR_MAX;
}
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
// h_pwm_duty_change(g_ptz.hori_pid.PidUT_uint);
h_pwm_duty_change(450);
//将当前PID输出值保存
g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float;
}
else if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP) {
g_speed_to_hall.hori_speed_t.startTime_60ms = TIMER_CNT(TIMER6);
g_speed_to_hall.hori_speed_t.startTime_us = g_speed_to_hall.time_60ms;
g_speed_to_hall.hori_speed_t.speed = 0;
g_ptz.hori_pid.LastError = 0;
g_speed_to_hall.hori_speed_pid.sum_error = 0;
g_ptz.hori_speed_actual = 0;
}
}
void get_vert_speed(void)
{
if(g_ptz.vert_start_stop_set == PTZ_HORI_START) {
/* hall数量未变，表示速度没有变化 */
static uint32_t hallNum;
static uint32_t hallLastNum;
hallNum = g_ptz.vert_pid.hall_h1_count + g_ptz.vert_pid.hall_h2_count + g_ptz.vert_pid.hall_h3_count;
if (hallLastNum != hallNum) {
g_ptz.vert_speed_actual = g_speed_to_hall.vert_speed_t.speed / PTZ_VERT_RATIO;
}
else {
g_ptz.vert_speed_actual = 0;
}
hallLastNum = hallNum;
//计算PID控制器输出值
// g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual) + g_ptz.vert_pid.LastUT_float;
g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual);
//控制PID的输出值增量，当前输出值与上一次输出值的差值必须在某个范围内
//即防止PID增量过大
//限定PID输出限定
if (fabs(g_ptz.vert_pid.PidUT_float - g_ptz.vert_pid.LastUT_float) > PTZ_VERT_PID_OUTPUT_LIMIT) {
if (g_ptz.vert_pid.PidUT_float > g_ptz.vert_pid.LastUT_float) {
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float + PTZ_VERT_PID_OUTPUT_LIMIT;
}
else {
g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.LastUT_float - PTZ_VERT_PID_OUTPUT_LIMIT;
}
}
g_ptz.vert_pid.PidUT_uint = (unsigned int)(g_ptz.vert_pid.PidUT_float + 0.5);
//限制PID的输出值在某个指定的范围
//限制输入模拟电压最小值
if (g_ptz.vert_pid.PidUT_uint <= PTZ_VERT_VR_MIN) {
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MIN;
}
//限制输入模拟电压最大值
if (g_ptz.vert_pid.PidUT_uint > PTZ_VERT_VR_MAX) {
g_ptz.vert_pid.PidUT_uint = PTZ_VERT_VR_MAX;
}
//将PID输出的电机转速模拟电压等级输入到模拟电压输出芯片
v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint);
//将当前PID输出值保存
g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float;
}
else if (g_ptz.vert_start_stop_set == PTZ_VERT_STOP) {
g_speed_to_hall.vert_speed_t.startTime_60ms = TIMER_CNT(TIMER6);
g_speed_to_hall.vert_speed_t.startTime_us = g_speed_to_hall.time_60ms;
g_speed_to_hall.vert_speed_t.speed = 0;
g_speed_to_hall.vert_speed_pid.sum_error = 0;
g_ptz.vert_pid.LastError = 0;
g_ptz.vert_pid.roll_start = 0;
g_ptz.vert_speed_actual = 0;
}
}

14
APP/Device/Device_speed/speed_to_bldc.h
View File

@ -230,21 +230,23 @@
//#define PTZ_VERT_PID_JY02A_SPEED //#define PTZ_VERT_PID_JY02A_SPEED
#define PTZ_PID_HALL_SPEED 1 #define PTZ_PID_HALL_SPEED 1
// #define PTZ_HALL_SPEED_SL 1
#define PTZ_HORI_PID_T 30u #define PTZ_HORI_PID_T 30u
#define PTZ_HORI_PID_HORI_KP 20.0//比例系数 #define PTZ_HORI_PID_HORI_KP 25.0 //比例系数
#define PTZ_HORI_PID_HORI_TI 100.0 //积分系数 #define PTZ_HORI_PID_HORI_TI 0.5 //积分系数
#define PTZ_HORI_PID_HORI_TD 0.0 //微分系数 #define PTZ_HORI_PID_HORI_TD 0.0 //微分系数
#define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID调速输入值限定 #define PTZ_HORI_PID_INPUT_LIMIT 100.0//PID调速输入值限定
#define PTZ_HORI_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 #define PTZ_HORI_PID_OUTPUT_LIMIT 400.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异
#define PTZ_VERT_PID_T 30u #define PTZ_VERT_PID_T 30u
#define PTZ_VERT_PID_VERT_KP 20.0//比例系数 #define PTZ_VERT_PID_VERT_KP 80.0 //比例系数
#define PTZ_VERT_PID_VERT_TI 100.0 //积分系数 #define PTZ_VERT_PID_VERT_TI 7.0 //积分系数
#define PTZ_VERT_PID_VERT_TD 0.0 //微分系数 #define PTZ_VERT_PID_VERT_TD 0.0 //微分系数
#define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定 #define PTZ_VERT_PID_INPUT_LIMIT 100.0//PID调速输入值限定
#define PTZ_VERT_PID_OUTPUT_LIMIT 200.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异 #define PTZ_VERT_PID_OUTPUT_LIMIT 300.0//PID调速输出值限定，当前输出值和上一次输出值之间的差异
#define PTZ_HORI_PID_T_MAX 100u//PID调速最大周期 #define PTZ_HORI_PID_T_MAX 100u//PID调速最大周期
#define PTZ_VERT_PID_T_MAX 100u//PID调速最大周期 #define PTZ_VERT_PID_T_MAX 100u//PID调速最大周期

90
APP/Device/device_Other/device_interrupt.c
View File

@ -8,6 +8,9 @@
#include "gd32f4xx_it.h" #include "gd32f4xx_it.h"
#include "ptz_type_select.h" #include "ptz_type_select.h"
#include "get_angle.h" #include "get_angle.h"
#include "speed_to_hall.h"
/// @brief 外部中断初始化 /// @brief 外部中断初始化
/// @param[in] usart_periph:EXTI_IRQ_init /// @param[in] usart_periph:EXTI_IRQ_init
/// @return none /// @return none
@ -593,6 +596,7 @@ void ptz_SW_IRQHandler(exti_line_enum sw_linex)
} }
uint32_t h_hall_time;
/// @brief 水平HALL中断处理函数 /// @brief 水平HALL中断处理函数
/// @param[in] hall_linex:EXTI_X（外部HALL中断线号EXTI_X） /// @param[in] hall_linex:EXTI_X（外部HALL中断线号EXTI_X）
/// @return none /// @return none
@ -604,6 +608,51 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex)
#ifdef PTZ_BLDC_MOTOR #ifdef PTZ_BLDC_MOTOR
#ifdef PTZ_HALL_FEEDBACK #ifdef PTZ_HALL_FEEDBACK
// if (g_speed_to_hall.hori_speed_t.flag == calculation_enable) {
// g_speed_to_hall.hori_speed_t.startTime_us = TIMER_CNT(TIMER6);
// g_speed_to_hall.hori_speed_t.startTime_60ms = g_speed_to_hall.time_60ms;
// g_speed_to_hall.hori_speed_t.hallNum = 0;
// g_speed_to_hall.hori_speed_t.flag = calculation_start;
// }
// if (g_speed_to_hall.hori_speed_t.flag == calculation_start) {
// g_speed_to_hall.hori_speed_t.hallNum++;
// }
// if (g_speed_to_hall.hori_speed_t.flag == calculation_ok) {
// g_speed_to_hall.hori_speed_t.endTime_us = TIMER_CNT(TIMER6);
// g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.time_60ms;
// g_speed_to_hall.hori_speed_t.flag = calculation_end;
// }
g_speed_to_hall.hori_speed_t.endTime_us = TIMER_CNT(TIMER6);
g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.time_60ms;
// uint32_t h_hall_time;
if (g_speed_to_hall.hori_speed_t.endTime_60ms > g_speed_to_hall.hori_speed_t.startTime_60ms) {
h_hall_time = (g_speed_to_hall.hori_speed_t.endTime_60ms - g_speed_to_hall.hori_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us);
}
else if (g_speed_to_hall.hori_speed_t.endTime_60ms = g_speed_to_hall.hori_speed_t.startTime_60ms) {
h_hall_time = g_speed_to_hall.hori_speed_t.endTime_us - g_speed_to_hall.hori_speed_t.startTime_us;
}
else {
h_hall_time = (g_speed_to_hall.hori_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.hori_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.hori_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.hori_speed_t.startTime_us);
}
// if (time > 200000) {
// g_speed_to_hall.hori_speed_t.speed = 0;
// }
// else {
// g_speed_to_hall.hori_speed_t.speed = 0.2 * (150000000.0 / (float)time) + 0.8 * g_speed_to_hall.hori_speed_t.lastSpeed;
// }
g_speed_to_hall.hori_speed_t.speed = 0.8 * (5000000.0 / (float)h_hall_time) + 0.2 * g_speed_to_hall.hori_speed_t.lastSpeed;
// g_speed_to_hall.hori_speed_t.speed = (5000000.0 / (float)h_hall_time);
g_speed_to_hall.hori_speed_t.lastSpeed = g_speed_to_hall.hori_speed_t.speed;
g_speed_to_hall.hori_speed_t.startTime_60ms = g_speed_to_hall.hori_speed_t.endTime_60ms;
g_speed_to_hall.hori_speed_t.startTime_us = g_speed_to_hall.hori_speed_t.endTime_us;
switch(hall_linex) switch(hall_linex)
{ {
case EXTI_10://水平HALL-1 case EXTI_10://水平HALL-1
@ -817,12 +866,51 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex)
/// @return none /// @return none
/// @note 修改日志 /// @note 修改日志
/// LH于2022-05-26 /// LH于2022-05-26
uint32_t v_hall_time;
void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex) void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex)
{ {
#ifdef PTZ_BLDC_MOTOR #ifdef PTZ_BLDC_MOTOR
#ifdef PTZ_HALL_FEEDBACK #ifdef PTZ_HALL_FEEDBACK
// if (g_speed_to_hall.vert_speed_t.flag == calculation_enable) {
// g_speed_to_hall.vert_speed_t.startTime_us = TIMER_CNT(TIMER6);
// g_speed_to_hall.vert_speed_t.startTime_60ms = g_speed_to_hall.time_60ms;
// g_speed_to_hall.vert_speed_t.hallNum = 0;
// g_speed_to_hall.vert_speed_t.flag = calculation_start;
// }
// if (g_speed_to_hall.vert_speed_t.flag == calculation_start) {
// g_speed_to_hall.vert_speed_t.hallNum++;
// }
// if (g_speed_to_hall.vert_speed_t.flag == calculation_ok) {
// g_speed_to_hall.vert_speed_t.endTime_us = TIMER_CNT(TIMER6);
// g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.time_60ms;
// g_speed_to_hall.vert_speed_t.flag = calculation_end;
// }
g_speed_to_hall.vert_speed_t.endTime_us = TIMER_CNT(TIMER6);
g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.time_60ms;
if (g_speed_to_hall.vert_speed_t.endTime_60ms > g_speed_to_hall.vert_speed_t.startTime_60ms) {
v_hall_time = (g_speed_to_hall.vert_speed_t.endTime_60ms - g_speed_to_hall.vert_speed_t.startTime_60ms - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us);
}
else if (g_speed_to_hall.vert_speed_t.endTime_60ms = g_speed_to_hall.vert_speed_t.startTime_60ms) {
v_hall_time = g_speed_to_hall.vert_speed_t.endTime_us - g_speed_to_hall.vert_speed_t.startTime_us;
}
else {
v_hall_time = (g_speed_to_hall.vert_speed_t.endTime_60ms + (TIME_60MS_MAX - g_speed_to_hall.vert_speed_t.startTime_60ms) - 1) * SPEED_TIMER_PERIOD
+ g_speed_to_hall.vert_speed_t.endTime_us + (SPEED_TIMER_PERIOD - g_speed_to_hall.vert_speed_t.startTime_us);
}
g_speed_to_hall.vert_speed_t.speed = 0.8 * (5000000.0 / (float)v_hall_time) + 0.2 * g_speed_to_hall.hori_speed_t.lastSpeed;
g_speed_to_hall.vert_speed_t.lastSpeed = g_speed_to_hall.vert_speed_t.speed;
g_speed_to_hall.vert_speed_t.startTime_60ms = g_speed_to_hall.vert_speed_t.endTime_60ms;
g_speed_to_hall.vert_speed_t.startTime_us = g_speed_to_hall.vert_speed_t.endTime_us;
switch(hall_linex) switch(hall_linex)
{ {

63
APP/Service/service_statusmonitor.c
View File

@ -13,30 +13,43 @@ static char ptz_temp_volt_current_fault_detect_task()
{//只报故障，不做响应 {//只报故障，不做响应
static unsigned short int time_ms; static unsigned short int time_ms;
time_ms ++; static uint8_t H_CURR_FAULT_NUM = 0;
if(time_ms < 50) static uint8_t V_CURR_FAULT_NUM = 0;
{
//轻型云台峰值电流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 ) time_ms ++;
{//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 if(time_ms < 50)
{
g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警 //轻型云台峰值电流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 )
ptz_hori_stop(PTZ_HORI_STOP_TIME); {//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。
if (5 < H_CURR_FAULT_NUM++) {
g_ptz.fault_detect.Phase_curr_H = FAULT;//水平电机相电流过大，报警
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
}
else {
H_CURR_FAULT_NUM = 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 )
{//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。
if (5 < V_CURR_FAULT_NUM++) {
g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
}
else {
V_CURR_FAULT_NUM = 0;
}
return 1;
} }
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 ) else
{//堵转检测，防止电机堵转烧坏，结合电机卡死故障监测，与云台工作电流监测。 {
time_ms = 0;
g_ptz.fault_detect.Phase_curr_V = FAULT;//垂直电机相电流过大，报警
ptz_vert_stop(PTZ_VERT_STOP_TIME);
} }
return 1;
}
else
{
time_ms = 0;
}
#ifdef Full_bridge //NTC温度监测，暂定温度上限值100摄氏度，后续测试修改 #ifdef Full_bridge //NTC温度监测，暂定温度上限值100摄氏度，后续测试修改
@ -287,7 +300,8 @@ static char ptz_motor_rotate_fault_detect_task()
{ {
time_ms = 0; time_ms = 0;
} }
return 1;
//垂直 //垂直
@ -534,7 +548,6 @@ static char ptz_motor_rotate_fault_detect_task()
} }
if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP)
{ {
vert_rotate_fault_step = 0;//关闭判断 vert_rotate_fault_step = 0;//关闭判断
@ -846,7 +859,7 @@ static char ptz_fault_detect_task()
ptz_temp_volt_current_fault_detect_task(); ptz_temp_volt_current_fault_detect_task();
//电机hall故障 //电机hall故障
#ifdef PTZ_BLDC_MOTOR #ifdef PTZ_BLDC_MOTOR
ptz_motor_hall_fault_detect_task(); // ptz_motor_hall_fault_detect_task();
#endif #endif
//故障回传 //故障回传
ptz_fault_return_task(); ptz_fault_return_task();

94
BSP/Driver/speed/speed_to_hall.c Normal file
View File

@ -0,0 +1,94 @@
#include "speed_to_hall.h"
speed_hall g_speed_to_hall = {0};
/**
* @brief 初始化定时器+霍尔计算速度
* @param
* @retval
*
*/
void init_hall_speed_module(void)
{
/* 计时中断 */
timer_parameter_struct timer_initpara;
rcu_periph_clock_enable(RCU_TIMER6);
timer_struct_para_init(&timer_initpara);
timer_deinit(TIMER6);
// TIMER1 configuration
timer_initpara.prescaler = 99;
timer_initpara.alignedmode = TIMER_COUNTER_EDGE;//计数模式，边缘对齐
timer_initpara.counterdirection = TIMER_COUNTER_UP;//计数方向
timer_initpara.period = SPEED_TIMER_PERIOD;//计数重装载值，确定周期，，该值可在任意时刻进行修改以改变周期，TIMER_CAR(timer_periph) = (uint32_t)initpara->period;
timer_initpara.clockdivision = TIMER_CKDIV_DIV1;//时钟分频
timer_initpara.repetitioncounter = 0;//计数重复值
timer_init(TIMER6, &timer_initpara);
timer_auto_reload_shadow_enable(TIMER6);
timer_enable(TIMER6);
timer_interrupt_enable(TIMER6, TIMER_INT_UP);//定时器更新中断使能
nvic_irq_enable(TIMER6_IRQn, 2U, 2U);
/* 控制中断 */
rcu_periph_clock_enable(RCU_TIMER12);
timer_struct_para_init(&timer_initpara);
timer_deinit(TIMER12);
// TIMER1 configuration
timer_initpara.prescaler = 99;
timer_initpara.alignedmode = TIMER_COUNTER_EDGE;//计数模式，边缘对齐
timer_initpara.counterdirection = TIMER_COUNTER_UP;//计数方向
timer_initpara.period = 19999;//计数重装载值，确定周期，，该值可在任意时刻进行修改以改变周期，TIMER_CAR(timer_periph) = (uint32_t)initpara->period;
timer_initpara.clockdivision = TIMER_CKDIV_DIV1;//时钟分频
timer_initpara.repetitioncounter = 0;//计数重复值
timer_init(TIMER12, &timer_initpara);
timer_auto_reload_shadow_enable(TIMER12);
timer_enable(TIMER12);
timer_interrupt_enable(TIMER12, TIMER_INT_UP);//定时器更新中断使能
nvic_irq_enable(TIMER7_UP_TIMER12_IRQn, 1U, 2U);
}
/**
* @brief 定时器6中断服务函数
* @param
* @retval
*
*/
void TIMER6_IRQHandler(void)
{
if(RESET != timer_interrupt_flag_get(TIMER6, TIMER_INT_FLAG_UP)) {
g_speed_to_hall.time_60ms++;
if (g_speed_to_hall.time_60ms >= TIME_60MS_MAX) {
g_speed_to_hall.time_60ms = 0;
}
timer_interrupt_flag_clear(TIMER6, TIMER_INT_FLAG_UP);
}
}
/**
* @brief 定时器7中断服务函数
* @param
* @retval
*
*/
void TIMER7_UP_TIMER12_IRQHandler(void)
{
if(RESET != timer_interrupt_flag_get(TIMER12, TIMER_INT_FLAG_UP)) {
get_hori_speed();
get_vert_speed();
timer_interrupt_flag_clear(TIMER12, TIMER_INT_FLAG_UP);
}
}

62
BSP/Driver/speed/speed_to_hall.h Normal file
View File

@ -0,0 +1,62 @@
#ifndef __BSP_DRIVER_SPEED_HALL_H_
#define __BSP_DRIVER_SPEED_HALL_H_
//#include "ptz_type_select.h"
#include "gd32f4xx.h"
// typedef struct _speed_t {
// uint32_t hallNum; //霍尔编码器次数（运行方向的）
// uint32_t startTime_60ms; //开始时间
// uint32_t startTime_us;
// uint32_t endTime_60ms; //结束时间
// uint32_t endTime_us;
// uint8_t flag; //标志位
// }speed_t;
typedef struct _speed_t {
uint32_t startTime_60ms; //开始时间
uint32_t startTime_us;
uint32_t endTime_60ms; //结束时间
uint32_t endTime_us;
float speed; //速度
float lastSpeed; //上一次速度
}speed_t;
typedef struct _speed_pid_t {
float kp;
float ki;
float kd;
float sum_error; //积分误差
}speed_pid_t;
typedef struct _speed_to_hall {
speed_t hori_speed_t;
speed_t vert_speed_t;
speed_pid_t hori_speed_pid;
speed_pid_t vert_speed_pid;
uint32_t time_60ms;
}speed_hall;
typedef enum _calculation_speed_type {
calculation_disable = 0, //未开始计算速度
calculation_enable = 1, //使能计算速度
calculation_ok = 2, //达到计算速度时间
calculation_start = 3, //开始计算速度
calculation_end = 4, //结束计算速度
}calculation_speed_type;
extern speed_hall g_speed_to_hall;
#define TIME_60MS_MAX 4000000000
#define SPEED_TIMER_PERIOD 60000
void init_hall_speed_module(void);
extern void get_hori_speed(void);
extern void get_vert_speed(void);
#endif

13
PROJECT/OS2.ewp
View File

@ -409,6 +409,7 @@
<state>$PROJ_DIR$\..\BSP\Driver\timer</state> <state>$PROJ_DIR$\..\BSP\Driver\timer</state>
<state>$PROJ_DIR$\..\BSP\Driver\full_bridge</state> <state>$PROJ_DIR$\..\BSP\Driver\full_bridge</state>
<state>$PROJ_DIR$\..\APP\Service</state> <state>$PROJ_DIR$\..\APP\Service</state>
<state>$PROJ_DIR$\..\BSP\Driver\speed</state>
</option> </option>
<option> <option>
<name>CCStdIncCheck</name> <name>CCStdIncCheck</name>
@ -825,7 +826,7 @@
</option> </option>
<option> <option>
<name>IlinkIcfFile</name> <name>IlinkIcfFile</name>
<state>D:\psx\Pan-Tilt\1.software\HY\4.0 LW21-01B\BSP\IAR\GD32F450xE_APP.icf</state> <state>D:\psx\Pan-Tilt\1.software\HY\4.0 LW21-01B\BSP\IAR\GD32F450xE.icf</state>
</option> </option>
<option> <option>
<name>IlinkIcfFileSlave</name> <name>IlinkIcfFileSlave</name>
@ -2622,6 +2623,7 @@
<state>$PROJ_DIR$\..\BSP\Driver\timer</state> <state>$PROJ_DIR$\..\BSP\Driver\timer</state>
<state>$PROJ_DIR$\..\BSP\Driver\full_bridge</state> <state>$PROJ_DIR$\..\BSP\Driver\full_bridge</state>
<state>$PROJ_DIR$\..\BSP\Driver\timer</state> <state>$PROJ_DIR$\..\BSP\Driver\timer</state>
<state>$PROJ_DIR$\..\BSP\Driver\speed</state>
</option> </option>
<option> <option>
<name>CCStdIncCheck</name> <name>CCStdIncCheck</name>
@ -2912,6 +2914,15 @@
<name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name> <name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name>
</file> </file>
</group> </group>
<group>
<name>speed</name>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\speed\speed_to_hall.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\speed\speed_to_hall.h</name>
</file>
</group>
<group> <group>
<name>timer</name> <name>timer</name>
<file> <file>

9
PROJECT/OS2.ewt
View File

@ -3182,6 +3182,15 @@
<name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name> <name>$PROJ_DIR$\..\BSP\Driver\ringqueue\ring_queue.h</name>
</file> </file>
</group> </group>
<group>
<name>speed</name>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\speed\speed_to_hall.c</name>
</file>
<file>
<name>$PROJ_DIR$\..\BSP\Driver\speed\speed_to_hall.h</name>
</file>
</group>
<group> <group>
<name>timer</name> <name>timer</name>
<file> <file>