//云台电子稳定，由于直齿轮云台自锁能力太差，增加电子自锁功能。 //当云台处于滑动状态时，给予反向力矩，防止云台转动 #include "ptz_header_file.h" //#include "device_rotate_electricstable.h" //#include "device_rotate.h" #ifdef PTZ_BLDC_MOTOR #ifdef PTZ_ELECTRIC_STABLE_L6235D //数据初始化 void ptz_hori_electric_stable_init() { g_ptz.hori_electric_stable.stable_stop_switch = 1; g_ptz.hori_electric_stable.sample_count = 0; g_ptz.hori_electric_stable.stable_sample_switch = 0; g_ptz.hori_electric_stable.slide_dir_change_num = 0; g_ptz.hori_electric_stable.stable_vref = 0; memset(g_ptz.hori_electric_stable.stable_angle, 0, sizeof(g_ptz.hori_electric_stable.stable_angle)); } void ptz_vert_electric_stable_init() { g_ptz.vert_electric_stable.stable_stop_switch = 1; g_ptz.vert_electric_stable.torque_switch = 0; g_ptz.vert_electric_stable.stop_hall_count = 0; // g_ptz.vert_electric_stable.sample_count = 0; // g_ptz.vert_electric_stable.stable_sample_switch = 0; g_ptz.vert_electric_stable.slide_dir_change_num = 0; g_ptz.vert_electric_stable.stable_vref = 0; g_ptz.vert_electric_stable.stop_hall_count = 0; g_ptz.vert_electric_stable.state = 0; // memset(g_ptz.vert_electric_stable.stable_angle, 0, sizeof(g_ptz.vert_electric_stable.stable_angle)); } static char ptz_vert_electric_stable_task() { if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP &&//判断是否处于刹车状态 g_ptz.vert_electric_stable.stable_stop_switch == 1 &&//刹车停止时间是否结束 g_ptz.vert_electric_stable.stop_hall_count > VERT_STOP_HALL_NUM)//刹车后霍尔脉冲满足多少个时 { //判断反向力矩方向 g_ptz.vert_electric_stable.slide_direction_actual = g_ptz.vert_direction_actual; if(g_ptz.vert_electric_stable.slide_direction_actual == PTZ_VERT_DIR_DOWN) {//如果云台向下滑动 g_ptz.vert_electric_stable.reverse_torque_direction = PTZ_VERT_DIR_UP; } //如果云台向上滑动 if(g_ptz.vert_electric_stable.slide_direction_actual == PTZ_VERT_DIR_UP) { g_ptz.vert_electric_stable.reverse_torque_direction = PTZ_VERT_DIR_DOWN; } if(g_ptz.vert_electric_stable.torque_switch == 1)//判断刹车PWM的占空比 { if(g_ptz.vert_electric_stable.reverse_torque_direction_a == g_ptz.vert_electric_stable.reverse_torque_direction) {//如果力矩没有改变方向，则继续增大解除刹车占空比 g_ptz.vert_electric_stable.torque_start_t = g_ptz.vert_electric_stable.torque_start_t + 1; if(g_ptz.vert_electric_stable.torque_start_t > TORQUE_TOTAL_T) { g_ptz.vert_electric_stable.torque_start_t = TORQUE_TOTAL_T; } g_ptz.vert_electric_stable.torque_stop_t = TORQUE_TOTAL_T - g_ptz.vert_electric_stable.torque_start_t; } else {//如果力矩方向改变，则减少解除刹车占空比 if(g_ptz.vert_electric_stable.torque_start_t > 0) { g_ptz.vert_electric_stable.torque_start_t = g_ptz.vert_electric_stable.torque_start_t - 1; } g_ptz.vert_electric_stable.torque_stop_t = TORQUE_TOTAL_T - g_ptz.vert_electric_stable.torque_start_t; } } if(g_ptz.vert_electric_stable.torque_switch == 0)//初始化反向力矩 { //保存初始力矩方向 g_ptz.vert_electric_stable.reverse_torque_direction_a = g_ptz.vert_electric_stable.reverse_torque_direction; //输入反向力矩方向 l6235d_vert_set_direction(g_ptz.vert_electric_stable.reverse_torque_direction); //输入指定模拟电压 g_ptz.vert_electric_stable.stable_vref = TORQUE_VREF; adc7311_vert_vr(g_ptz.vert_electric_stable.stable_vref); //计算刹车PWM波的初始刹车时间和解除刹车初始时间 g_ptz.vert_electric_stable.torque_start_t = TORQUE_TOTAL_T / 2; g_ptz.vert_electric_stable.torque_stop_t = TORQUE_TOTAL_T - g_ptz.vert_electric_stable.torque_start_t; g_ptz.vert_electric_stable.torque_switch = 1; } g_ptz.vert_electric_stable.reverse_torque_direction_last = g_ptz.vert_electric_stable.reverse_torque_direction;//保存上一次反向力矩的方向 g_ptz.vert_electric_stable.stop_hall_count --; } return 1; } static void ptz_vert_electric_stable_pwm_task() { while(1) { static char flag; if(g_ptz.vert_start_stop_set == PTZ_VERT_START && flag == 1) {//防止反向力矩PWM波干扰电机正常启动 flag = 0; l6235d_vert_start_stop(PTZ_VERT_START); } if(g_ptz.vert_start_stop_set == PTZ_VERT_STOP && g_ptz.vert_electric_stable.torque_switch == 1) {//提供刹车用的PWM波 if(g_ptz.vert_electric_stable.torque_start_t > 0) { l6235d_vert_start_stop(PTZ_VERT_START);//启动 OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_electric_stable.torque_start_t); } if(g_ptz.vert_electric_stable.torque_stop_t > 0) { l6235d_vert_start_stop(PTZ_VERT_STOP); //刹车 OSTimeDlyHMSM(0u, 0u, 0u, g_ptz.vert_electric_stable.torque_stop_t); } flag = 1; } else { OSTimeDlyHMSM(0u, 0u, 0u, STABLE_T); } } } //#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO 17u //#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE 100u static OS_STK task_vert_electric_stable_pwm_stk[TASK_VERT_ELECTRIC_STABLE_PWM_SIZE]; static void creat_task_vert_electric_stable_pwm(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_electric_stable_pwm_task, (void *) 0, (OS_STK *)&task_vert_electric_stable_pwm_stk[TASK_VERT_ELECTRIC_STABLE_PWM_SIZE - 1], (INT8U ) TASK_VERT_ELECTRIC_STABLE_PWM_PRIO, (INT16U ) TASK_VERT_ELECTRIC_STABLE_PWM_PRIO, (OS_STK *)&task_vert_electric_stable_pwm_stk[0], (INT32U ) TASK_VERT_ELECTRIC_STABLE_PWM_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_VERT_ELECTRIC_STABLE_PWM_PRIO, "ptz_vert_electric_stable_pwm_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_electric_stable_pwm_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_electric_stable_pwm_task failed...\n\r"); } } static void ptz_electric_stable_task() { while(1) { ptz_vert_electric_stable_task(); //ptz_hori_electric_stable_task(); g_ptz.hori_electric_stable.state = 0; OSTimeDlyHMSM(0u, 0u, 0u, STABLE_T); g_ptz.hori_electric_stable.state = 1; } } static OS_STK task_electric_stable_stk[TASK_ELECTRIC_STABLE_STK_SIZE]; static void creat_task_electric_stable(void) { CPU_INT08U task_err; CPU_INT08U name_err; task_err = OSTaskCreateExt((void (*)(void *)) ptz_electric_stable_task, (void *) 0, (OS_STK *)&task_electric_stable_stk[TASK_ELECTRIC_STABLE_STK_SIZE - 1], (INT8U ) TASK_ELECTRIC_STABLE_PRIO, (INT16U ) TASK_ELECTRIC_STABLE_PRIO, (OS_STK *)&task_electric_stable_stk[0], (INT32U ) TASK_ELECTRIC_STABLE_STK_SIZE, (void *) 0, (INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR)); #if (OS_TASK_NAME_EN > 0) OSTaskNameSet(TASK_ELECTRIC_STABLE_PRIO, "ptz_electric_stable_task", &name_err); #endif if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) { pdebug(DEBUG_LEVEL_INFO,"create ptz_electric_stable_task success...\n\r"); } else { pdebug(DEBUG_LEVEL_FATAL,"create ptz_electric_stable_task failed...\n\r"); } } #endif #endif void task_electric_stable_init(void) { #ifdef PTZ_BLDC_MOTOR #ifdef PTZ_ELECTRIC_STABLE_ON creat_task_electric_stable(); #ifdef PTZ_ELECTRIC_STABLE_L6235D creat_task_vert_electric_stable_pwm(); #endif #endif #endif }