pengsx
/
MW22-02A
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
MW22-02A/APP/Device/Device_rotate/rotate_electricstable.c

220 lines
7.8 KiB
C
Raw Permalink Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
//<2F><>̨<EFBFBD><CCA8><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̫<EFBFBD><EFBFBD><EEA3AC><EFBFBD>ӵ<EFBFBD><D3B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܡ<EFBFBD>
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD><CCA8><EFBFBD>ڻ<EFBFBD><DABB><EFBFBD>״̬ʱ<CCAC><CAB1><EFBFBD><EFBFBD><EFBFBD>跴<EFBFBD><E8B7B4><EFBFBD><EFBFBD><EFBFBD>أ<EFBFBD><D8A3><EFBFBD>ֹ<EFBFBD><D6B9>̨ת<CCA8><D7AA>
#include "ptz_header_file.h"
//#include "device_rotate_electricstable.h"
//#include "device_rotate.h"
#ifdef PTZ_BLDC_MOTOR
#ifdef PTZ_ELECTRIC_STABLE_L6235D
//<2F><><EFBFBD>ݳ<EFBFBD>ʼ<EFBFBD><CABC>
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 &&//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>״̬
g_ptz.vert_electric_stable.stable_stop_switch == 1 &&//ɲ<><C9B2>ֹͣʱ<D6B9><CAB1><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_electric_stable.stop_hall_count > VERT_STOP_HALL_NUM)//ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ٸ<EFBFBD>ʱ
{
//<2F>жϷ<D0B6><CFB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ط<EFBFBD><D8B7><EFBFBD>
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)
{//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8><EFBFBD>»<EFBFBD><C2BB><EFBFBD>
g_ptz.vert_electric_stable.reverse_torque_direction = PTZ_VERT_DIR_UP;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̨<EFBFBD><CCA8><EFBFBD>ϻ<EFBFBD><CFBB><EFBFBD>
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)//<2F>ж<EFBFBD>ɲ<EFBFBD><C9B2>PWM<57><4D>ռ<EFBFBD>ձ<EFBFBD>
{
if(g_ptz.vert_electric_stable.reverse_torque_direction_a ==
g_ptz.vert_electric_stable.reverse_torque_direction)
{//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD>иı䷽<C4B1><E4B7BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ռ<EFBFBD>ձ<EFBFBD>
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
{//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ط<EFBFBD><D8B7><EFBFBD><EFBFBD>ı䣬<C4B1><E4A3AC><EFBFBD><EFBFBD><EFBFBD>ٽ<EFBFBD><D9BD><EFBFBD>ɲ<EFBFBD><C9B2>ռ<EFBFBD>ձ<EFBFBD>
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)//<2F><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
{
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC><EFBFBD>ط<EFBFBD><D8B7><EFBFBD>
g_ptz.vert_electric_stable.reverse_torque_direction_a =
g_ptz.vert_electric_stable.reverse_torque_direction;
//<2F><><EFBFBD>뷴<EFBFBD><EBB7B4><EFBFBD><EFBFBD><EFBFBD>ط<EFBFBD><D8B7><EFBFBD>
l6235d_vert_set_direction(g_ptz.vert_electric_stable.reverse_torque_direction);
//<2F><><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD>ѹ
g_ptz.vert_electric_stable.stable_vref = TORQUE_VREF;
adc7311_vert_vr(g_ptz.vert_electric_stable.stable_vref);
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>PWM<57><4D><EFBFBD>ij<EFBFBD>ʼɲ<CABC><C9B2>ʱ<EFBFBD><CAB1><EFBFBD>ͽ<EFBFBD><CDBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD>ʼʱ<CABC><CAB1>
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;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>һ<EFBFBD>η<EFBFBD><CEB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>صķ<D8B5><C4B7><EFBFBD>
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)
{//<2F><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>PWM<57><4D><EFBFBD><EFBFBD><EFBFBD>ŵ<EFBFBD><C5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
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)
{//<2F>ṩɲ<E1B9A9><C9B2><EFBFBD>õ<EFBFBD>PWM<57><4D>
if(g_ptz.vert_electric_stable.torque_start_t > 0)
{
l6235d_vert_start_stop(PTZ_VERT_START);//<2F><><EFBFBD><EFBFBD>
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); //ɲ<><C9B2>
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
}