220 lines
7.8 KiB
C
220 lines
7.8 KiB
C
//云台电子稳定，由于直齿轮云台自锁能力太差，增加电子自锁功能。
|
|
//当云台处于滑动状态时，给予反向力矩，防止云台转动
|
|
#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
|
|
} |