diff --git a/APP/Appcfg/app.c b/APP/Appcfg/app.c index e0eeff7..0129c58 100644 --- a/APP/Appcfg/app.c +++ b/APP/Appcfg/app.c @@ -198,7 +198,7 @@ static void task_start (void *p_arg) init_angle_module(); // term_printf("\n angle init \r\n\r\n"); -// task_printf_init(); + task_printf_init(); //自检模块初始化 init_self_check_module(); @@ -269,20 +269,23 @@ int main (void) */ static void task_print() { - OS_TCB *p_tcb; - OS_STK_DATA stkDat; - p_tcb = &OSTCBTbl[0]; +// OS_TCB *p_tcb; +// OS_STK_DATA stkDat; +// p_tcb = &OSTCBTbl[0]; - char cpuusage = OSCPUUsage; +// char cpuusage = OSCPUUsage; - term_printf(" Prio Used Free Per cpuusage TaskName\r\n"); - while(p_tcb != NULL) - { - 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); - p_tcb = p_tcb->OSTCBPrev; - } - term_printf("\r\n"); +// term_printf(" Prio Used Free Per cpuusage TaskName\r\n"); +// while(p_tcb != NULL) +// { +// 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); +// p_tcb = p_tcb->OSTCBPrev; +// } +// term_printf("\r\n"); + term_printf("h_angle:%f, v_angle:%f\n", as5047d_hori_get_angle_a(), as5047d_vert_get_angle_a()); + + OSTimeDlyHMSM(0u, 0u, 1u, 0u); } diff --git a/APP/Common/ptz_default_value.h b/APP/Common/ptz_default_value.h index f4721ce..9a6fce1 100644 --- a/APP/Common/ptz_default_value.h +++ b/APP/Common/ptz_default_value.h @@ -3,333 +3,333 @@ #include "ptz_type_select.h" #ifdef PTZ_LIGHT_GEAR_L6235D_AS5047D - ///ˮƽٱ + ///水平电机减速比 #define PTZ_HORI_MOTOR_RATIO 100.0 - ///ˮƽּٱ + ///水平大齿轮减速比 #define PTZ_HORI_BIG_GEAR_RATIO 4.05 - ///ˮƽܼٱ + ///水平总减速比 #define PTZ_HORI_RATIO 405.0 - ///ˮƽģѹֵ + ///水平电机调速模拟电压最大值 #define PTZ_HORI_VR_MAX 1999 - ///ˮƽģѹСֵ + ///水平电机调速模拟电压最小值 #define PTZ_HORI_VR_MIN 0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 400.0//600.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 7.5 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 1.0//1.5//0.65 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #define PTZ_HORI_BEST_SPEED 5.0//4.0 /**********************************************************/ - ///ű͵֮ļٱ + ///磁编码器和电机之间的减速比 #define PTZ_VERT_MOTOR_RATIO 200.0 - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 7.00 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 1400.0 - ///ֱģѹֵ + ///垂直电机调速模拟电压最大值 #define PTZ_VERT_VR_MAX 1999 - ///ֱģѹСֵ + ///垂直电机调速模拟电压最小值 #define PTZ_VERT_VR_MIN 0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 300.0 - ///ֱ̨ת + ///垂直云台最大转速 #define PTZ_VERT_MAX_SPEED 2.20 - ///ֱ̨Сת + ///垂直云台最小转速 #define PTZ_VERT_MIN_SPEED 0.22//0.43 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 1.7//1.8 - //ż + //电机磁极对数 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 - //ż + //电机磁极对数 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #endif #ifdef PTZ_MEDIUM_WORM_L6235D_AS5047D -///*******************************׼̨******************************/ - ///ˮƽٱ +///*******************************标准云台******************************/ + ///水平电机减速比 #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_RATIO 2000.0//2325//1240.0 - ///ˮƽģѹֵ + ///水平电机调速模拟电压最大值 #define PTZ_HORI_VR_MAX 1999 - ///ˮƽģѹСֵ + ///水平电机调速模拟电压最小值 #define PTZ_HORI_VR_MIN 0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.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_MIN_SPEED 0.2//0.3//0.26//0.4 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #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_BIG_GEAR_RATIO 80.0//54.00 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 6000.0//4011.429//1080.0 - ///ֱģѹֵ + ///垂直电机调速模拟电压最大值 #define PTZ_VERT_VR_MAX 1999 - ///ֱģѹСֵ + ///垂直电机调速模拟电压最小值 #define PTZ_VERT_VR_MIN 0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.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_MIN_SPEED 0.07//0.1//0.15//0.5 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 0.5//0.7//2.0 - //ż + //电机磁极对数 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 - //ż + //电机磁极对数 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 -/*******************************̨******************************/ -// ///ˮƽٱ +/*******************************激光云台定制******************************/ +// ///水平电机减速比 // #define PTZ_HORI_MOTOR_RATIO (100.0 * 1.5) -// ///ˮƽּٱ +// ///水平大齿轮减速比 // #define PTZ_HORI_BIG_GEAR_RATIO 62.0 -// ///ˮƽܼٱ +// ///水平总减速比 // #define PTZ_HORI_RATIO 9300 -// ///ˮƽģѹֵ +// ///水平电机调速模拟电压最大值 // #define PTZ_HORI_VR_MAX 4095 -// ///ˮƽģѹСֵ +// ///水平电机调速模拟电压最小值 // #define PTZ_HORI_VR_MIN 0 // -// ///תٵλ ת/ÿ -// ///ˮƽת +// ///转速单位 转/每分 +// ///水平电机最大转速 // #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 -// ///ˮƽСת +// ///水平电机最小转速 // #define PTZ_HORI_MOTOR_MIN_SPEED 600.0 -// ///ˮƽ̨ת +// ///水平云台最大转速 // #define PTZ_HORI_MAX_SPEED 0.32 -// ///ˮƽ̨Сת +// ///水平云台最小转速 // #define PTZ_HORI_MIN_SPEED 0.06 -// ///ˮƽ̨Ĭٶ +// ///水平云台默认最佳速度 // #define PTZ_HORI_BEST_SPEED 0.32 // // /**********************************************************/ // -// ///ű͵֮ļٱ +// ///磁编码器和电机之间的减速比 // #define PTZ_VERT_MOTOR_RATIO (50.0 * (52.0 / 35.0)) -// ///ų֮̋ļٱ +// ///磁编码器与云台轴之间的减速比 // #define PTZ_VERT_BIG_GEAR_RATIO 54.00 -// ///ֱܼٱ +// ///垂直总减速比 // #define PTZ_VERT_RATIO 4011.429 -// ///ֱģѹֵ +// ///垂直电机调速模拟电压最大值 // #define PTZ_VERT_VR_MAX 4095 -// ///ֱģѹСֵ +// ///垂直电机调速模拟电压最小值 // #define PTZ_VERT_VR_MIN 0 // -// ///תٵλ ת/ÿ -// ///ֱת +// ///转速单位 转/每分 +// ///垂直电机最大转速 // #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 -// ///ֱСת +// ///垂直电机最小转速 // #define PTZ_VERT_MOTOR_MIN_SPEED 600.0 -// ///ֱ̨ת +// ///垂直云台最大转速 // #define PTZ_VERT_MAX_SPEED 0.74 -// ///ֱ̨Сת +// ///垂直云台最小转速 // #define PTZ_VERT_MIN_SPEED 0.15 -// ///ֱ̨Ĭٶ +// ///垂直云台默认最佳速度 // #define PTZ_VERT_BEST_SPEED 0.7 // // -// //ż +// //电机磁极对数 // #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 -// // +// //电机霍尔传感器对数 // #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 -// //תһȦFGصĻ +// //电机旋转一圈FG返回的霍尔脉冲数 // #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 // -// //ż +// //电机磁极对数 // #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 -// // +// //电机霍尔传感器对数 // #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 -// //תһȦFGصĻ +// //电机旋转一圈FG返回的霍尔脉冲数 // #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #endif #ifdef PTZ_HEAVY_WORM_L6235D_AS5047D - ///ˮƽٱ************************************׼ + ///水平电机减速比************************************标准 #define PTZ_HORI_MOTOR_RATIO 25.0//(25.0 * 1.0) - ///ˮƽּٱ + ///水平大齿轮减速比 #define PTZ_HORI_BIG_GEAR_RATIO 52.0 - ///ˮƽܼٱ + ///水平总减速比 #define PTZ_HORI_RATIO 1300.0 - ///ˮƽģѹֵ + ///水平电机调速模拟电压最大值 #define PTZ_HORI_VR_MAX 1999 - ///ˮƽģѹСֵ + ///水平电机调速模拟电压最小值 #define PTZ_HORI_VR_MIN 0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 3000.0 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 300.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 2.3 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 0.2 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #define PTZ_HORI_BEST_SPEED 2.1 - ///ű͵֮ļٱ************************************׼ + ///磁编码器和电机之间的减速比************************************标准 #define PTZ_VERT_MOTOR_RATIO 125.8065//(50.0 * (78.0 / 31.0)) - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 60.0 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 7548.3871 - ///ֱģѹֵ + ///垂直电机调速模拟电压最大值 #define PTZ_VERT_VR_MAX 1999 - ///ֱģѹСֵ + ///垂直电机调速模拟电压最小值 #define PTZ_VERT_VR_MIN 0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 3000.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 600.0 - ///ֱ̨ת + ///垂直云台最大转速 #define PTZ_VERT_MAX_SPEED 0.4 - ///ֱ̨Сת + ///垂直云台最小转速 #define PTZ_VERT_MIN_SPEED 0.08 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 0.30 - //ż + //电机磁极对数 #define PTZ_HORI_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_HORI_ONE_CYCLE_HALL_NUM 15.0 - //ż + //电机磁极对数 #define PTZ_VERT_MOTOR_POLE_PAIRS 5.0 - // + //电机霍尔传感器对数 #define PTZ_VERT_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 15.0 #endif #ifdef PTZ_SUPER_LIGHT_WORM_L6235D_AS5047D_12V -/*****************************ڶ汾**********************************/ - ///ˮƽٱ +/*****************************第二个版本**********************************/ + ///水平电机减速比 #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 0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 2000//2000.0 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 1.48//1.48 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 0.3 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #define PTZ_HORI_BEST_SPEED 1.2 /**********************************************************/ - ///ű͵֮ļٱ + ///磁编码器和电机之间的减速比 #define PTZ_VERT_MOTOR_RATIO 27.0//19.2//27.0 - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 50.0 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 1350.0//960.0//1350.0 - ///ֱģѹֵ + ///垂直电机调速模拟电压最大值 #define PTZ_VERT_VR_MAX 1999 - ///ֱģѹСֵ + ///垂直电机调速模拟电压最小值 #define PTZ_VERT_VR_MIN 0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 2000.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 405.0 - ///ֱ̨ת + ///垂直云台最大转速 #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_BEST_SPEED 1.2//1.8//1.2 - //ż + //电机磁极对数 #define PTZ_HORI_MOTOR_POLE_PAIRS 4.0 - // + //电机霍尔传感器对数 #define PTZ_HORI_MOTOR_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈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صĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0 #endif @@ -338,223 +338,223 @@ #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_MAX_SPEED 3000 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 405.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 2.2 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 0.3 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #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_MIN_SPEED 405.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 20.0 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 2.7 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #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_MAX_SPEED 3000.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 405.0 - ///ֱ̨ת + ///垂直云台最大转速 #define PTZ_VERT_MAX_SPEED 2.2 - ///ֱ̨Сת + ///垂直云台最小转速 #define PTZ_VERT_MIN_SPEED 0.3 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 1.5 #endif #ifdef NEW_MOTOR - ///ű͵֮ļٱ + ///磁编码器和电机之间的减速比 #define PTZ_VERT_MOTOR_RATIO 10.0 - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 50.0 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 500.0 - ///ֱģѹֵ + ///垂直电机调速模拟电压最大值 #define PTZ_VERT_VR_MAX 1899 - ///ֱģѹСֵ + ///垂直电机调速模拟电压最小值 #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_HALL_PAIRS 3.0 - //תһȦFGصĻ + //电机旋转一圈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صĻ + //电机旋转一圈FG返回的霍尔脉冲数 #define PTZ_VERT_ONE_CYCLE_HALL_NUM 12.0 #endif -// +//步进电机 #ifdef PTZ_SUPER_LIGHT_WORM_STEP_TMC2160_AS5047D_24V - ///ˮƽٱ + ///水平电机减速比 #define PTZ_HORI_MOTOR_RATIO 1.0 - ///ˮƽּٱ + ///水平大齿轮减速比 #define PTZ_HORI_BIG_GEAR_RATIO 50.0 - ///ˮƽܼٱ + ///水平总减速比 #define PTZ_HORI_RATIO 50.0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 750.0 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 1.0 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 15.0 - ///ˮƽ̨Сת + ///水平云台最小转速 #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_VERT_MOTOR_RATIO 1.0 - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 50.0 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 50.0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 500//300.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 1.0 - ///ֱ̨ת + ///垂直云台最大转速 #define PTZ_VERT_MAX_SPEED 10.0 - ///ֱ̨Сת + ///垂直云台最小转速 #define PTZ_VERT_MIN_SPEED 0.02 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 2.0//6.0 - ///Ҫָλʱɲת + ///快要到达指定位置时的刹车转速 #define PTZ_VERT_BREAK_SPEED 0.3 ///////////////////**////////////////////////////// - ///Ĭϲ + ///步进电机默认步距角 #define PTZ_HORI_MOTOR_STEP 1.8 - ///Ĭϲ + ///步进电机默认步距角 #define PTZ_VERT_MOTOR_STEP 1.8 #endif #ifdef PTZ_MEDIUM_WORM_STEP_TMC2160_AS5047D_24V - ///ˮƽٱ + ///水平电机减速比 #define PTZ_HORI_MOTOR_RATIO 1.0 - ///ˮƽּٱ + ///水平大齿轮减速比 #define PTZ_HORI_BIG_GEAR_RATIO 80.0 - ///ˮƽܼٱ + ///水平总减速比 #define PTZ_HORI_RATIO 80.0 - ///תٵλ ת/ÿ - ///ˮƽת + ///转速单位 转/每分 + ///水平电机最大转速 #define PTZ_HORI_MOTOR_MAX_SPEED 500.0 - ///ˮƽСת + ///水平电机最小转速 #define PTZ_HORI_MOTOR_MIN_SPEED 1.6 - ///ˮƽ̨ת + ///水平云台最大转速 #define PTZ_HORI_MAX_SPEED 6.25 - ///ˮƽ̨Сת + ///水平云台最小转速 #define PTZ_HORI_MIN_SPEED 0.02 - ///ˮƽ̨Ĭٶ + ///水平云台默认最佳速度 #define PTZ_HORI_BEST_SPEED 1.5 - ///Ҫָλʱɲת + ///快要到达指定位置时的刹车转速 #define PTZ_HORI_BREAK_SPEED 0.3 /**********************************************************/ - ///ű͵֮ļٱ + ///磁编码器和电机之间的减速比 #define PTZ_VERT_MOTOR_RATIO 1.5 - ///ų֮̋ļٱ + ///磁编码器与云台轴之间的减速比 #define PTZ_VERT_BIG_GEAR_RATIO 80.0 - ///ֱܼٱ + ///垂直总减速比 #define PTZ_VERT_RATIO 120.0 - ///תٵλ ת/ÿ - ///ֱת + ///转速单位 转/每分 + ///垂直电机最大转速 #define PTZ_VERT_MOTOR_MAX_SPEED 300.0 - ///ֱСת + ///垂直电机最小转速 #define PTZ_VERT_MOTOR_MIN_SPEED 2.4 - ///ֱ̨ת + ///垂直云台最大转速 #define PTZ_VERT_MAX_SPEED 2.5 - ///ֱ̨Сת + ///垂直云台最小转速 #define PTZ_VERT_MIN_SPEED 0.02 - ///ֱ̨Ĭٶ + ///垂直云台默认最佳速度 #define PTZ_VERT_BEST_SPEED 0.8 - ///Ҫָλʱɲת + ///快要到达指定位置时的刹车转速 #define PTZ_VERT_BREAK_SPEED 0.3 ///////////////////**////////////////////////////// - ///Ĭϲ + ///步进电机默认步距角 #define PTZ_HORI_MOTOR_STEP 1.8 - ///Ĭϲ + ///步进电机默认步距角 #define PTZ_VERT_MOTOR_STEP 1.8 #endif diff --git a/APP/Device/Device_rotate/rotate_bldc.c b/APP/Device/Device_rotate/rotate_bldc.c index 737e74a..a847db1 100644 --- a/APP/Device/Device_rotate/rotate_bldc.c +++ b/APP/Device/Device_rotate/rotate_bldc.c @@ -827,7 +827,7 @@ static char ptz_vert_rotate_monitor_task() case PTZ_VERT_DEC_BRAKE_B: k ++;//K1,ʱ1ms if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 2 || - k >= 500) + k >= 100) { k = 0; g_ptz.vert_rotate_monitor_switch = 0; diff --git a/APP/Device/Device_rotate/rotate_bldc.h b/APP/Device/Device_rotate/rotate_bldc.h index 154a92d..8ccea85 100644 --- a/APP/Device/Device_rotate/rotate_bldc.h +++ b/APP/Device/Device_rotate/rotate_bldc.h @@ -224,16 +224,16 @@ #define PTZ_VERT_DIR_STOP 2 ///ɲСתеľ - #define PTZ_HORI_MIN_SPEED_ANGLE 5.0 - #define PTZ_VERT_MIN_SPEED_ANGLE 5.0 + #define PTZ_HORI_MIN_SPEED_ANGLE 6.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_VERT_STOP_NEAR_DISTANCE 0.01 + #define PTZ_VERT_STOP_NEAR_DISTANCE 0.1 ///ɲԶ #define PTZ_VERT_STOP_FAR_DISTANCE 4.0 diff --git a/APP/Device/Device_speed/speed_to_bldc.c b/APP/Device/Device_speed/speed_to_bldc.c index 8a8db1f..52b3dfb 100644 --- a/APP/Device/Device_speed/speed_to_bldc.c +++ b/APP/Device/Device_speed/speed_to_bldc.c @@ -11,6 +11,7 @@ #ifdef PTZ_BLDC_MOTOR + //̨ʵת void ptz_send_speed(char dev, char speed) { @@ -83,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.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_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() @@ -96,7 +105,7 @@ void ptz_hori_pid_clear_zero() // hori_dac0_data_out(0); } -static float ptz_hori_pid_calculate(float H_SampSpeed, uint32_t H_SampTime) +static float ptz_hori_pid_calculate(float H_SampSpeed) { float H_IError,H_IIncPid; H_IError = g_ptz.hori_speed_set - H_SampSpeed;//ǰPID_INPUT_LIMIT @@ -111,7 +120,19 @@ static float ptz_hori_pid_calculate(float H_SampSpeed, uint32_t H_SampTime) // } // // 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 = PTZ_HORI_PID_HORI_KP * (H_IError - g_ptz.hori_pid.LastError) + PTZ_HORI_PID_HORI_TI * H_IError * H_SampTime; + // 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; @@ -130,6 +151,11 @@ static void ptz_hori_pid_task() { unsigned int time = 0; char i = 0; + + + uint32_t startTime; + uint32_t endTime; + while(1) { if(g_ptz.hori_start_stop_set == PTZ_HORI_START) @@ -623,63 +649,90 @@ static void ptz_hori_pid_task() // h_pwm_duty_change(PTZ_HORI_VR_MIN); // } - /* ʹܼٶ */ - g_speed_to_hall.hori_speed_t.flag = calculation_enable; +// /* ʹܼٶ */ +// g_speed_to_hall.hori_speed_t.flag = calculation_enable; - /* ʱ */ - uint32_t timeOut = 0; +// /* ʱ */ +// 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жٶȼ */ +// 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; +// /* ʱhallźٶ */ +// OSTimeDlyHMSM(0u, 0u, 0u, 50u); +// g_speed_to_hall.hori_speed_t.flag = calculation_ok; - timeOut = 0; +// 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; - } +// /* ȴһ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); - } +// 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; +// g_ptz.hori_speed_actual = (float)g_speed_to_hall.hori_speed_t.hallNum * 5000000.0 / (float)time / PTZ_HORI_RATIO; -timeOutErr: - ; +// 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) {//ٶȴ趨ٶȵ3 g_ptz.hori_pid.PidUT_float = g_ptz.hori_pid.PidUT_float * 0.7;////ֱ߼ @@ -690,7 +743,7 @@ timeOutErr: else { //PIDֵ - g_ptz.hori_pid.PidUT_float = ptz_hori_pid_calculate(g_ptz.hori_speed_actual, time) + 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ֵǰֵһֵIJֵijΧ //ֹPID @@ -731,7 +784,8 @@ timeOutErr: //PIDĵתģѹȼ뵽ģѹоƬ // 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ֵ g_ptz.hori_pid.LastUT_float = g_ptz.hori_pid.PidUT_float; @@ -739,10 +793,18 @@ timeOutErr: 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; OSTimeDlyHMSM(0u, 0u, 0u, 10u); } - +#endif + } + #ifdef PTZ_PID_HALL_SPEED // //ֹݳ if(g_ptz.hori_pid.hall_h1_count >= UNSIGNED_INT_MAX_VALUE) @@ -759,6 +821,7 @@ timeOutErr: } #endif + OSTimeDlyHMSM(0u, 0u, 0u, 50u); } } @@ -809,7 +872,7 @@ void ptz_vert_pid_clear_zero() // vert_dac1_data_out(0); } -static float ptz_vert_pid_calculate(float SampSpeed, uint32_t H_SampTime) +static float ptz_vert_pid_calculate(float SampSpeed) { float IError,IIncPid; IError = g_ptz.vert_speed_set - SampSpeed;//ǰPID_INPUT_LIMIT @@ -826,7 +889,19 @@ static float ptz_vert_pid_calculate(float SampSpeed, uint32_t H_SampTime) // // 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 = PTZ_VERT_PID_VERT_KP * (IError - g_ptz.hori_pid.LastError) + PTZ_VERT_PID_VERT_TI * IError * H_SampTime; + + 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; @@ -844,9 +919,9 @@ static void ptz_vert_pid_task() char i = 0; 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 //ű٣0 g_ptz.vert_as5047d.as5047d_speed_angle_a = 0; @@ -1354,11 +1429,11 @@ timeOutErr: #endif - } - //PID - if(g_ptz.vert_start_stop_set == PTZ_VERT_START && - g_ptz.vert_pid.mode == PID_ADJUST_SPEED) - { + // } + // //PID + // if(g_ptz.vert_start_stop_set == PTZ_VERT_START && + // g_ptz.vert_pid.mode == PID_ADJUST_SPEED) + // { #ifdef PTZ_CONTROL_SPEED_L6235D //жǷҪ @@ -1398,63 +1473,63 @@ timeOutErr: // g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; // } // else - if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2) - {//ٶȴ趨ٶȵ3 - g_ptz.vert_pid.PidUT_float = g_ptz.vert_pid.PidUT_float - 100.0;//* 1.3/*PTZ_HORI_PID_ADD*/; +// if(g_ptz.vert_speed_actual > g_ptz.vert_speed_set * 2) +// {//ٶȴ趨ٶȵ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.PrevError = g_ptz.vert_pid.LastError; - g_ptz.vert_pid.LastError = g_ptz.vert_speed_set - g_ptz.vert_speed_actual; - } - else - { - //PIDֵ - g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual, time) + g_ptz.vert_pid.LastUT_float; +// 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; +// } +// else +// { +// //PIDֵ +// g_ptz.vert_pid.PidUT_float = ptz_vert_pid_calculate(g_ptz.vert_speed_actual, time) + g_ptz.vert_pid.LastUT_float; - //PIDֵǰֵһֵIJֵijΧ - //ֹ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) - { - 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; - } - } - } +// //PIDֵǰֵһֵIJֵijΧ +// //ֹ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) +// { +// 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; +// } +// } +// } - if(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)//ģѹֵ - { - g_ptz.vert_pid.PidUT_float = PTZ_VERT_VR_MAX; - } +// if(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)//ģѹֵ +// { +// 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ֵijָķΧ - 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ֵijָķΧ +// 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ĵתģѹȼ뵽ģѹоƬ -// vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint); +// //PIDĵתģѹȼ뵽ģѹоƬ +// // vert_dac1_data_out( g_ptz.vert_pid.PidUT_uint); - v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); - //ǰPIDֵ - g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float; - } +// v_pwm_duty_change(g_ptz.vert_pid.PidUT_uint); +// //ǰPIDֵ +// g_ptz.vert_pid.LastUT_float = g_ptz.vert_pid.PidUT_float; +// } @@ -1518,12 +1593,12 @@ timeOutErr: } #endif - if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) - { - g_ptz.vert_pid.roll_start = 0; - g_ptz.vert_speed_actual = 0; - OSTimeDlyHMSM(0u, 0u, 0u, 10u); - } + // if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP) + // { + // g_ptz.vert_pid.roll_start = 0; + // g_ptz.vert_speed_actual = 0; + // OSTimeDlyHMSM(0u, 0u, 0u, 10u); + // } #ifdef PTZ_PID_HALL_SPEED // //ֹݳ @@ -1541,6 +1616,7 @@ timeOutErr: } #endif + OSTimeDlyHMSM(0u, 0u, 0u, 50u); } } @@ -1660,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ֵǰֵһֵIJֵijΧ + //ֹ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ֵijָķΧ + //ģѹСֵ + 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(600); + + //ǰ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ֵǰֵһֵIJֵijΧ + //ֹ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ֵijָķΧ + //ģѹСֵ + 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; + } +} diff --git a/APP/Device/Device_speed/speed_to_bldc.h b/APP/Device/Device_speed/speed_to_bldc.h index 3d69ba8..9dadf7d 100644 --- a/APP/Device/Device_speed/speed_to_bldc.h +++ b/APP/Device/Device_speed/speed_to_bldc.h @@ -230,23 +230,23 @@ //#define PTZ_VERT_PID_JY02A_SPEED #define PTZ_PID_HALL_SPEED 1 - #define PTZ_HALL_SPEED_SL 1 + // #define PTZ_HALL_SPEED_SL 1 #define PTZ_HORI_PID_T 30u - #define PTZ_HORI_PID_HORI_KP 35.0//ϵ - #define PTZ_HORI_PID_HORI_TI 0.0003 //ϵ + #define PTZ_HORI_PID_HORI_KP 50.0 //ϵ + #define PTZ_HORI_PID_HORI_TI 2.5 //ϵ #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ֵ޶ǰֵһֵ֮IJ + #define PTZ_HORI_PID_OUTPUT_LIMIT 400.0//PIDֵ޶ǰֵһֵ֮IJ #define PTZ_VERT_PID_T 30u - #define PTZ_VERT_PID_VERT_KP 40.0//ϵ - #define PTZ_VERT_PID_VERT_TI 0.0001 //ϵ + #define PTZ_VERT_PID_VERT_KP 80.0 //ϵ + #define PTZ_VERT_PID_VERT_TI 7.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ֵ޶ǰֵһֵ֮IJ + #define PTZ_VERT_PID_OUTPUT_LIMIT 300.0//PIDֵ޶ǰֵһֵ֮IJ #define PTZ_HORI_PID_T_MAX 100u//PID #define PTZ_VERT_PID_T_MAX 100u//PID diff --git a/APP/Device/device_Other/device_interrupt.c b/APP/Device/device_Other/device_interrupt.c index 8cbc71d..9b95f6a 100644 --- a/APP/Device/device_Other/device_interrupt.c +++ b/APP/Device/device_Other/device_interrupt.c @@ -596,6 +596,7 @@ void ptz_SW_IRQHandler(exti_line_enum sw_linex) } + uint32_t h_hall_time; /// @brief ˮƽHALLжϴ /// @param[in] hall_linex:EXTI_XⲿHALLжߺEXTI_X /// @return none @@ -607,22 +608,50 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex) #ifdef PTZ_BLDC_MOTOR #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_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 (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; - } + // 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) { @@ -837,28 +866,50 @@ void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex) /// @return none /// @note ޸־ /// LH2022-05-26 +uint32_t v_hall_time; void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex) { #ifdef PTZ_BLDC_MOTOR #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_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); } - 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.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) { diff --git a/BSP/Driver/speed/speed_to_hall.c b/BSP/Driver/speed/speed_to_hall.c index 5250c5a..a3690ec 100644 --- a/BSP/Driver/speed/speed_to_hall.c +++ b/BSP/Driver/speed/speed_to_hall.c @@ -12,6 +12,7 @@ speed_hall g_speed_to_hall = {0}; */ void init_hall_speed_module(void) { + /* 计时中断 */ timer_parameter_struct timer_initpara; rcu_periph_clock_enable(RCU_TIMER6); @@ -33,12 +34,29 @@ void init_hall_speed_module(void) timer_interrupt_enable(TIMER6, TIMER_INT_UP);//定时器更新中断使能 nvic_irq_enable(TIMER6_IRQn, 2U, 2U); - g_speed_to_hall.hori_speed_t.flag = calculation_disable; - g_speed_to_hall.vert_speed_t.flag = calculation_disable; + /* 控制中断 */ + 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 定时器1中断服务函数 + * @brief 定时器6中断服务函数 * @param * @retval * @@ -54,6 +72,21 @@ void TIMER6_IRQHandler(void) } } +/** + * @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); + } +} diff --git a/BSP/Driver/speed/speed_to_hall.h b/BSP/Driver/speed/speed_to_hall.h index 5c52792..d9ffdfc 100644 --- a/BSP/Driver/speed/speed_to_hall.h +++ b/BSP/Driver/speed/speed_to_hall.h @@ -5,18 +5,36 @@ #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 hallNum; //霍尔编码器次数（运行方向的） uint32_t startTime_60ms; //开始时间 uint32_t startTime_us; uint32_t endTime_60ms; //结束时间 uint32_t endTime_us; - uint8_t flag; //标志位 + 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; @@ -28,6 +46,8 @@ typedef enum _calculation_speed_type { calculation_end = 4, //结束计算速度 }calculation_speed_type; + + extern speed_hall g_speed_to_hall; #define TIME_60MS_MAX 4000000000 @@ -35,6 +55,8 @@ extern speed_hall g_speed_to_hall; void init_hall_speed_module(void); +extern void get_hori_speed(void); +extern void get_vert_speed(void); #endif