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

1814 lines
58 KiB
C
Raw Normal View History

汇源原始工程
2025-05-21 01:55:40 +00:00
#include "ptz_header_file.h"
#include "Timer.h"
#include "ptz_default_value.h"
#include "ptz_type_select.h"
#include "rotate_step.h"
#include "speed_to_step.h"
#ifdef PTZ_STEP_MOTOR
static BSP_OS_SEM ptz_hori_stop_mutex;//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
static BSP_OS_SEM ptz_vert_stop_mutex;
static char ptz_hori_stop_count;//ˮƽֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
static char ptz_vert_stop_count;//<2F><>ֱֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ
void ptz_sem_post_stop_mutex()//<2F>ͷ<EFBFBD><CDB7><EFBFBD>̨<EFBFBD><CCA8>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>
{
BSP_OS_SemPost(&ptz_hori_stop_mutex);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
//ת<><D7AA>r/minת<6E><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>,step ϸ<>̶ֳ<D6B3>1,2,4,8,16,32,64,128,256
float ptz_hori_step_v_to_f(float v, unsigned short int microstep_alue)
{
float f;
f = v * 6.0 * (float)microstep_alue / PTZ_HORI_MOTOR_STEP;
return f;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>ת<EFBFBD><D7AA>Ϊת<CEAA><D7AA>r/min
float ptz_hori_step_f_to_v(float f, unsigned short int microstep_alue)
{
float v;
v = f * PTZ_HORI_MOTOR_STEP / ((float)microstep_alue * 6.0);
return v;
}
//ת<><D7AA>r/minת<6E><D7AA>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>,step ϸ<>̶ֳ<D6B3>1,2,4,8,16,32,64,128,256
float ptz_vert_step_v_to_f(float v, unsigned short int microstep_alue)
{
float f;
f = v * 6.0 * (float)microstep_alue / PTZ_VERT_MOTOR_STEP;
return f;
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>ת<EFBFBD><D7AA>Ϊת<CEAA><D7AA>r/min
float ptz_vert_step_f_to_v(float f, unsigned short int microstep_alue)
{
float v;
v = f * PTZ_VERT_MOTOR_STEP / ((float)microstep_alue * 6.0);
return v;
}
#ifdef TMC2160
extern TMC2160Parameter tmc2160_parameter;
float ptz_hori_break_angle()
{
float s,s1,s2,v2;
s1 = 3.0 * PTZ_HORI_BREAK_SPEED * PTZ_HORI_BREAK_SPEED * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0;
v2 = ptz_hori_step_f_to_v(/*g_ptz.hori_drv8711.set_f*/g_ptz.hori_tmc2160.f, g_ptz.hori_tmc2160.microstep_vlue) / PTZ_HORI_RATIO;
s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.hori_dec_time_max / PTZ_HORI_MAX_SPEED / 1000.0;
s = s2 -s1;
g_ptz.hori_tmc2160.stop_angle = s;
return s;
}
float ptz_vert_break_angle()
{
float s,s1,s2,v2;
s1 = 3.0 * PTZ_VERT_BREAK_SPEED * PTZ_VERT_BREAK_SPEED * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0;
v2 = ptz_vert_step_f_to_v(/*g_ptz.vert_drv8711.set_f*/g_ptz.vert_tmc2160.f, g_ptz.vert_tmc2160.microstep_vlue) / PTZ_VERT_RATIO;
s2 = 3.0 * v2 * v2 * (float)tmc2160_parameter.vert_dec_time_max / PTZ_VERT_MAX_SPEED / 1000.0;
s = s2 -s1;
g_ptz.vert_tmc2160.stop_angle = s;
return s;
}
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD>жϲ<D0B6><CFB2>õ<EFBFBD>ϸ<EFBFBD>̶ֳ<D6B3>,<2C><><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>v<EFBFBD><76><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ת<EFBFBD><D7AA>
TMC2160Control ptz_hori_choice_microstep(float ptz_v)
{
float motor_v;
TMC2160Control data_tmc2160;
motor_v = ptz_v * PTZ_HORI_RATIO;
if(motor_v > PTZ_HORI_MOTOR_MAX_SPEED)
{
motor_v = PTZ_HORI_MOTOR_MAX_SPEED;
}
if(motor_v < PTZ_HORI_MOTOR_MIN_SPEED)
{
motor_v = PTZ_HORI_MOTOR_MIN_SPEED;
}
data_tmc2160.set_v = motor_v;
//<2F><>һ<EFBFBD><D2BB>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>ֵ
data_tmc2160.microstep = TMC2160_1_16_STEP;
data_tmc2160.microstep_vlue = 16;
if(tmc2160_parameter.hori_step_1_256_en == 1 &&
motor_v >= tmc2160_parameter.hori_step_1_256_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_256_v_max)
{
data_tmc2160.microstep = TMC2160_1_256_STEP;
data_tmc2160.microstep_vlue = 256;
}
if(tmc2160_parameter.hori_step_1_128_en == 1 &&
motor_v >= tmc2160_parameter.hori_step_1_128_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_128_v_max)
{
data_tmc2160.microstep = TMC2160_1_128_STEP;
data_tmc2160.microstep_vlue = 128;
}
if(tmc2160_parameter.hori_step_1_64_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_64_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_64_v_max)
{
data_tmc2160.microstep = TMC2160_1_64_STEP;
data_tmc2160.microstep_vlue = 64;
}
if(tmc2160_parameter.hori_step_1_32_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_32_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_32_v_max)
{
data_tmc2160.microstep = TMC2160_1_32_STEP;
data_tmc2160.microstep_vlue = 32;
}
if(tmc2160_parameter.hori_step_1_16_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_16_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_16_v_max)
{
data_tmc2160.microstep = TMC2160_1_16_STEP;
data_tmc2160.microstep_vlue = 16;
}
if(tmc2160_parameter.hori_step_1_8_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_8_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_8_v_max)
{
data_tmc2160.microstep = TMC2160_1_8_STEP;
data_tmc2160.microstep_vlue = 8;
}
if(tmc2160_parameter.hori_step_1_4_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_4_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_4_v_max)
{
data_tmc2160.microstep = TMC2160_1_4_STEP;
data_tmc2160.microstep_vlue = 4;
}
if(tmc2160_parameter.hori_step_1_2_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_2_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_2_v_max)
{
data_tmc2160.microstep = TMC2160_1_2_STEP;
data_tmc2160.microstep_vlue = 2;
}
if(tmc2160_parameter.hori_step_1_1_en == 1 &&
motor_v > tmc2160_parameter.hori_step_1_1_v_min &&
motor_v <= tmc2160_parameter.hori_step_1_1_v_max)
{
data_tmc2160.microstep = TMC2160_1_1_STEP;
data_tmc2160.microstep_vlue = 1;
}
switch(data_tmc2160.microstep)
{
case TMC2160_1_1_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 1);
break;
case TMC2160_1_2_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 2);
break;
case TMC2160_1_4_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 4);
break;
case TMC2160_1_8_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 8);
break;
case TMC2160_1_16_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 16);
break;
case TMC2160_1_32_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 32);
break;
case TMC2160_1_64_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 64);
break;
case TMC2160_1_128_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 128);
break;
case TMC2160_1_256_STEP:
data_tmc2160.set_f = ptz_hori_step_v_to_f(motor_v, 256);
break;
}
return data_tmc2160;
}
TMC2160Control ptz_vert_choice_microstep(float ptz_v)
{
float motor_v;
TMC2160Control data_tmc2160;
motor_v = ptz_v * PTZ_VERT_RATIO;
if(motor_v > PTZ_VERT_MOTOR_MAX_SPEED)
{
motor_v = PTZ_VERT_MOTOR_MAX_SPEED;
}
if(motor_v < PTZ_VERT_MOTOR_MIN_SPEED)
{
motor_v = PTZ_VERT_MOTOR_MIN_SPEED;
}
data_tmc2160.set_v = motor_v;
//<2F><>һ<EFBFBD><D2BB>Ԥ<EFBFBD><D4A4><EFBFBD><EFBFBD>Ĭ<EFBFBD><C4AC>ֵ
data_tmc2160.microstep = TMC2160_1_16_STEP;
data_tmc2160.microstep_vlue = 16;
if(tmc2160_parameter.vert_step_1_256_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_256_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_256_v_max)
{
data_tmc2160.microstep = TMC2160_1_256_STEP;
data_tmc2160.microstep_vlue = 256;
}
if(tmc2160_parameter.vert_step_1_128_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_128_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_128_v_max)
{
data_tmc2160.microstep = TMC2160_1_128_STEP;
data_tmc2160.microstep_vlue = 128;
}
if(tmc2160_parameter.vert_step_1_64_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_64_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_64_v_max)
{
data_tmc2160.microstep = TMC2160_1_64_STEP;
data_tmc2160.microstep_vlue = 64;
}
if(tmc2160_parameter.vert_step_1_32_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_32_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_32_v_max)
{
data_tmc2160.microstep = TMC2160_1_32_STEP;
data_tmc2160.microstep_vlue = 32;
}
if(tmc2160_parameter.vert_step_1_16_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_16_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_16_v_max)
{
data_tmc2160.microstep = TMC2160_1_16_STEP;
data_tmc2160.microstep_vlue = 16;
}
if(tmc2160_parameter.vert_step_1_8_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_8_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_8_v_max)
{
data_tmc2160.microstep = TMC2160_1_8_STEP;
data_tmc2160.microstep_vlue = 8;
}
if(tmc2160_parameter.vert_step_1_4_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_4_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_4_v_max)
{
data_tmc2160.microstep = TMC2160_1_4_STEP;
data_tmc2160.microstep_vlue = 4;
}
if(tmc2160_parameter.vert_step_1_2_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_2_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_2_v_max)
{
data_tmc2160.microstep = TMC2160_1_2_STEP;
data_tmc2160.microstep_vlue = 2;
}
if(tmc2160_parameter.vert_step_1_1_en == 1 &&
motor_v >= tmc2160_parameter.vert_step_1_1_v_min &&
motor_v <= tmc2160_parameter.vert_step_1_1_v_max)
{
data_tmc2160.microstep = TMC2160_1_1_STEP;
data_tmc2160.microstep_vlue = 1;
}
switch(data_tmc2160.microstep)
{
case TMC2160_1_1_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 1);
break;
case TMC2160_1_2_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 2);
break;
case TMC2160_1_4_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 4);
break;
case TMC2160_1_8_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 8);
break;
case TMC2160_1_16_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 16);
break;
case TMC2160_1_32_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 32);
break;
case TMC2160_1_64_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 64);
break;
case TMC2160_1_128_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 128);
break;
case TMC2160_1_256_STEP:
data_tmc2160.set_f = ptz_vert_step_v_to_f(motor_v, 256);
break;
}
return data_tmc2160;
}
void ptz_hori_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
g_ptz.hori_dec_mode = PTZ_DEC_MAX;
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
g_ptz.hori_speed_set = speed;
//<2F>趨ת<E8B6A8><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_last = g_ptz.hori_direction_set;
g_ptz.hori_direction_set = direction;
g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
tmc2160_set_hori_dir(direction);
//<2F><><EFBFBD><EFBFBD>оƬ<D0BE>Ĵ<EFBFBD><C4B4><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
g_ptz.hori_tmc2160.f = 0;
tmc2160_hori_motor_enable(TMC2160_MOTOR_ENABLE);
g_ptz.hori_tmc2160.off_on =1;
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
g_ptz.hori_start_stop_set = PTZ_HORI_START;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
ptz_hori_stop_count = 0;
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
g_ptz.hori_dec_mode = PTZ_DEC_MAX;
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
g_ptz.hori_speed_set = 0;
g_ptz.hori_speed_actual = 0;
tmc2160_hori_motor_enable(TMC2160_MOTOR_DISABLE);
g_ptz.hori_tmc2160.off_on = 0;
ptz_hori_timer_stop();
g_ptz.hori_tmc2160.f = 0;
g_ptz.hori_speed_actual = 0;
g_ptz.hori_tmc2160.off_on = 0;
//ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
//#ifdef PTZ_ELECTRIC_STABLE_ON//<2F>Ƿ<EFBFBD><C7B7>򿪵<EFBFBD><F2BFAAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// drv8711_hori_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.hori_self_lock_torque);
// drv8711_hori_change_register(DRV8711_CTRL_MODE, drv8711_parameter.hori_self_lock_micr);
// g_ptz.hori_drv8711.actual_microstep = drv8711_parameter.hori_self_lock_micr;
// drv8711_hori_enbl(DRV8711_MOTOR_ENABLE);
//#endif
if(ptz_hori_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_hori_stop_count = 0;
}
ptz_hori_stop_count ++;
BSP_OS_SemPost(&ptz_hori_stop_mutex);
}
/// <20><>̨ˮƽת<C6BD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static char ptz_hori_rotate_monitor_task()
{
static float BNP_D;//ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
static float BFP_D;//ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
static unsigned int k;
TMC2160Control data;
switch(g_ptz.hori_rotate_monitor_switch)
{
case 0://<2F>ر<EFBFBD>
break;
case PTZ_HORI_RIGHT_KEEP://<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
//<2F>ر<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_rotate_monitor_switch = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
// if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//ϸ<><CFB8><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ֱ<EFBFBD>Ӹ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// }else{
//
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//<2F>ȸ<EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD>ٸ<EFBFBD>ֵ<EFBFBD>ٶȣ<D9B6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>иı<D0B8><C4B1>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3>
// g_ptz.hori_tmc2160 = data;
// tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
// g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
// }
}else{
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
}
// //<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
// //<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
// if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT &&
// g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
// }
// else//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
// {
// //<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
//
// if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
// {
// g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
// g_ptz.hori_dec_mode = PTZ_DEC_DEF;
// }
// for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
// {
// if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
// {
// break;
// }
// OSTimeDlyHMSM(0u, 0u, 0u, 2u);
// }
//
// ptz_hori_stop(PTZ_HORI_STOP_TIME);
// //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
// ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
// }
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
}
break;
case PTZ_HORI_LEFT_KEEP://<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
//<2F>ر<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_rotate_monitor_switch = 0;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD>̨<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
// if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//ϸ<><CFB8><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ֱ<EFBFBD>Ӹ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// }else{
// g_ptz.hori_tmc2160 = data;
// tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//<2F>ȸ<EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD>ٸ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
// }
}
else//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
}
// //<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
// //<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
// if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT &&
// g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
//// g_ptz.hori_tmc2160 = data;
// }
// else//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
// {
// //<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
// if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
// {
// g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
// g_ptz.hori_dec_mode = PTZ_DEC_DEF;
// }
// for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
// {
// if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
// {
// break;
// }
// OSTimeDlyHMSM(0u, 0u, 0u, 2u);
// }
//
// ptz_hori_stop(PTZ_HORI_STOP_TIME);
// //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
// ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
// }
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
}
break;
case PTZ_HORI_GO_TO_ANGLE_A://<2F>Ƕȶ<C7B6>λ
case PTZ_HORI_RIGHT_ANGLE:
case PTZ_HORI_LEFT_ANGLE:
case PTZ_HORI_MIN_DISTANCE:
case PTZ_HORI_MAX_DISTANCE:
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
{
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
{
// if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//ϸ<><CFB8><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ֱ<EFBFBD>Ӹ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// }else{
// g_ptz.hori_tmc2160 = data;
// tmc2160_write_register(TMC2160_HORI, TMC2160_CHOPCONF, g_ptz.hori_tmc2160.microstep);
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//<2F>ȸ<EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD>ٸ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
// }
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
//data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
}
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
{
g_ptz.hori_rotate_monitor_switch = 0;
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
g_ptz.hori_arrive_flag = 0;//ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
}
break;
case PTZ_HORI_RIGHT_CYCLE:
case PTZ_HORI_LEFT_CYCLE:
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE;
break;
case PTZ_HORI_CYCLE:
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
if(BNP_D > 360.0 / 2.0)
{
BNP_D = 360.0 - BNP_D;
}
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
if(BFP_D > 360.0 / 2.0)
{
BFP_D = 360.0 - BFP_D;
}
/*<2A><>λ*/
if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0))
{
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
}
break;
case PTZ_HORI_GO_TO_ANGLE_B:
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
if(BNP_D > 360.0 / 2.0)
{
BNP_D = 360.0 - BNP_D;
}
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
if(BFP_D > 360.0 / 2.0)
{
BFP_D = 360.0 - BFP_D;
}
/*<2A><>λ*/
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
{
if(g_ptz.hori_repeat_locate_switch == 0)//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD>ر<EFBFBD>
{
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
}
g_ptz.hori_rotate_monitor_switch = 0;
if(g_ptz.hori_repeat_locate_switch == 1)//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD>
{
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A;
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
//data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
break;
case PTZ_HORI_BRAKE://ֹͣת<D6B9><D7AA>
g_ptz.hori_rotate_monitor_switch = 0;
ptz_hori_stop(PTZ_HORI_STOP_TIME);
break;
case PTZ_HORI_DEC_BRAKE_A://<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
//g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED);
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_MAX;
}
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B;
k = 0;
break;
case PTZ_HORI_DEC_BRAKE_B:
k++;//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1ms
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1 ||
k >= (tmc2160_parameter.hori_dec_time_max / 1.0))
{
k = 0;
g_ptz.hori_rotate_monitor_switch = 0;
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
break;
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ·<CEBB><C2B7><EFBFBD>滮
case PTZ_HORI_REPEAT_LOCATE_A:
ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B;
break;
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case PTZ_HORI_REPEAT_LOCATE_B:
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
{
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction &&
g_ptz.hori_tmc2160.actual_microstep == data.microstep)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
//data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
}
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
{
g_ptz.hori_rotate_monitor_switch = 0;
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
}
break;
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ<EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7>ﵽ<EFBFBD><EFB5BD>λλ<CEBB><CEBB>
case PTZ_HORI_REPEAT_LOCATE_C:
BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
if(BNP_D > 360.0 / 2.0)
{
BNP_D = 360.0 - BNP_D;
}
BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
if(BFP_D > 360.0 / 2.0)
{
BFP_D = 360.0 - BFP_D;
}
/*<2A><>λ*/
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
{
g_ptz.hori_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
g_ptz.hori_rotate_monitor_switch = 0;
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
//data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
break;
case PTZ_HORI_RIGHT_ANGLE_INC:
data = ptz_hori_choice_microstep(PTZ_HORI_BREAK_SPEED);
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD><C7B7><EFBFBD>Ҫת<D2AA><D7AA>,<2C><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0<CEAA><30><EFBFBD><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA>
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
{
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>ͬһ<CDAC><D2BB>λ<EFBFBD><CEBB>
//<2F>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction &&
g_ptz.hori_tmc2160.actual_microstep == data.microstep)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
g_ptz.hori_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_HORI_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.hori_speed_actual <= PTZ_HORI_BREAK_SPEED * 1.1)
{
break;
}
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_hori_stop(PTZ_HORI_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
g_ptz.hori_tmc2160 = data;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
}
//data = ptz_choice_microstep(PTZ_HORI_BREAK_SPEED);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
{
if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
{
if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
{
if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_BREAK_SPEED_ANGLE)
{
if(g_ptz.hori_speed_set > PTZ_HORI_BREAK_SPEED)
{
g_ptz.hori_speed_set = PTZ_HORI_BREAK_SPEED;
}
}
}
}
}
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
{
g_ptz.hori_rotate_monitor_switch = 0;
//<2F><><EFBFBD><EFBFBD>Ҫת<D2AA><D7AA><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>ֹͣ<CDA3><D6B9><EFBFBD>رռ<D8B1><D5BC><EFBFBD>
g_ptz.hori_arrive_flag = 0;//ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
ptz_location_return_return(LOCATION_HORI);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
{
ptz_hori_stop(PTZ_HORI_STOP_TIME);
}
}
break;
case PTZ_HORI_LEFT_ANGLE_INC:
break;
}
return 1;
}
void ptz_vert_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
g_ptz.vert_dec_mode = PTZ_DEC_MAX;
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
g_ptz.vert_speed_set = speed;
//<2F>趨ת<E8B6A8><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_direction_last = g_ptz.vert_direction_set;
g_ptz.vert_direction_set = direction;
g_ptz.vert_direction_actual = g_ptz.vert_direction_set;
tmc2160_set_vert_dir(direction);
//д<>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep);
g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep;
g_ptz.vert_tmc2160.f = 0;
tmc2160_vert_motor_enable(TMC2160_MOTOR_ENABLE);
g_ptz.vert_tmc2160.off_on = 1;
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
g_ptz.vert_start_stop_set = PTZ_VERT_START;
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
ptz_vert_stop_count = 0;
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_vert_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
g_ptz.vert_dec_mode = PTZ_DEC_MAX;
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
//<2F>趨ת<E8B6A8><D7AA><EFBFBD>ٶ<EFBFBD>
g_ptz.vert_speed_set = 0;
g_ptz.vert_speed_actual = 0;
//<2F><><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>״̬
tmc2160_vert_motor_enable(TMC2160_MOTOR_DISABLE);
g_ptz.vert_tmc2160.off_on = 0;
ptz_vert_timer_stop();
g_ptz.vert_tmc2160.f = 0;
g_ptz.vert_speed_actual = 0;
g_ptz.vert_tmc2160.off_on = 0;
//ֹͣ<CDA3><D6B9><EFBFBD><EFBFBD>
g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
//#ifdef PTZ_ELECTRIC_STABLE_ON//<2F>Ƿ<EFBFBD><C7B7>򿪵<EFBFBD><F2BFAAB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// drv8711_vert_change_register(DRV8711_TORQUE_TORQUE, drv8711_parameter.vert_self_lock_torque);
// drv8711_vert_change_register(DRV8711_CTRL_MODE, drv8711_parameter.vert_self_lock_micr);
// g_ptz.vert_drv8711.actual_microstep = drv8711_parameter.vert_self_lock_micr;
// drv8711_vert_enbl(DRV8711_MOTOR_ENABLE);
//#endif
if(ptz_vert_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_vert_stop_count = 0;
}
ptz_vert_stop_count ++;
}
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
/// <20><>̨<EFBFBD><CCA8>ֱת<D6B1><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
static char ptz_vert_rotate_monitor_task()
{
static float BNP_D;//ɲ<><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
static float BFP_D;//ɲ<><C9B2><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD>뵱ǰλ<C7B0>õľ<C3B5><C4BE><EFBFBD>
static char cmd_type;
static unsigned int k;
TMC2160Control data;
switch(g_ptz.vert_rotate_monitor_switch)
{
case 0://<2F>ر<EFBFBD>
break;
case PTZ_VERT_GO_TO_ANGLE_A://ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD>
case PTZ_VERT_ANGLE:
case PTZ_VERT_UP_KEEP:
case PTZ_VERT_DOWN_KEEP:
data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed);
cmd_type = g_ptz.vert_rotate_monitor_switch;
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B;
if(g_ptz.vert_rotate_plan.rotate_switch == 1)
{
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD>Ҳ<EFBFBD><D2B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
{
// g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
// if(g_ptz.vert_tmc2160.actual_microstep == data.microstep)
// {//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ϸ<EFBFBD>̶ֳ<D6B3><CCB6><EFBFBD>ͬ
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//ϸ<><CFB8><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC>ֱ<EFBFBD>Ӹ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// }else{
// g_ptz.vert_tmc2160 = data;
// tmc2160_write_register(TMC2160_VERT, TMC2160_CHOPCONF, g_ptz.vert_tmc2160.microstep);
// g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//<2F>ȸ<EFBFBD>ϸ<EFBFBD><CFB8><EFBFBD>ٸ<EFBFBD>ֵ<EFBFBD>ٶ<EFBFBD>
// g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep;
// }
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
g_ptz.vert_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1)
{
break;
}
#ifdef PTZ_PHOTOELECTRIC_SWITCH
if(g_ptz.vert_ps_sw1_state == PS_COVER)//<2F>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
{
break;
}
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
{
break;
}
}
#endif
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_vert_stop(PTZ_VERT_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
g_ptz.vert_tmc2160 = data;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
g_ptz.vert_tmc2160 = data;
}
}
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
g_ptz.vert_tmc2160 = data;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
g_ptz.vert_tmc2160 = data;
}
}
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
}
}
else//g_ptz.vert_rotate_plan.rotate_switch == 0
{
//ɲ<><C9B2><EFBFBD>ر<EFBFBD>ת<EFBFBD><D7AA>
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
g_ptz.vert_rotate_monitor_switch = 0;
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
}
break;
case PTZ_VERT_GO_TO_ANGLE_B://ת<><D7AA><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>Ƕȶ<C7B6>λ
BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
/*<2A><>λ*/
if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
{
if(g_ptz.vert_repeat_locate_switch == 0 &&
cmd_type != PTZ_VERT_UP_KEEP &&
cmd_type != PTZ_VERT_DOWN_KEEP)
{
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>Ԥλ<D4A4><CEBB>
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
}
g_ptz.vert_rotate_monitor_switch = 0;
if(g_ptz.vert_repeat_locate_switch == 1 &&
cmd_type != PTZ_VERT_UP_KEEP &&
cmd_type != PTZ_VERT_DOWN_KEEP)
{
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A;
}
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
//data = ptz_choice_microstep(PTZ_VERT_BREAK_SPEED);
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
//g_ptz.vert_drv8711.set_f = data.set_f;
}
}
break;
case PTZ_VERT_BRAKE://ɲ<><C9B2>
g_ptz.vert_rotate_monitor_switch = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
break;
case PTZ_VERT_DEC_BRAKE_A://<2F><>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
g_ptz.vert_dec_mode = PTZ_DEC_MAX;
}
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B;
k = 0;
break;
case PTZ_VERT_DEC_BRAKE_B:
k ++;//K<><4B><EFBFBD><EFBFBD>1,ʱ<><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>1ms
if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1 ||
k >= (tmc2160_parameter.vert_dec_time_max / 1.0))
{
k = 0;
g_ptz.vert_rotate_monitor_switch = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
break;
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λ·<CEBB><C2B7><EFBFBD>滮
case PTZ_VERT_REPEAT_LOCATE_A:
ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B;
break;
//<2F>ظ<EFBFBD><D8B8><EFBFBD>λת<CEBB><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
case PTZ_VERT_REPEAT_LOCATE_B:
data = ptz_vert_choice_microstep(g_ptz.vert_rotate_plan.max_speed);
if(g_ptz.vert_rotate_plan.rotate_switch == 1)
{
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C;
//<2F><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6>Ƿ<EFBFBD>ת<EFBFBD><D7AA>
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA>
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
//<2F>ж<EFBFBD>ת<EFBFBD><D7AA><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>ͬ
//<2F><><EFBFBD><EFBFBD>ת<EFBFBD><D7AA><EFBFBD><EFBFBD>ͬ
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction &&
g_ptz.vert_tmc2160.actual_microstep == data.microstep)
{
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
}
else//ת<><D7AA><EFBFBD><EFBFBD>ͬ<EFBFBD><CDAC><EFBFBD><EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>޸<EFBFBD><DEB8><EFBFBD><EFBFBD>õ<EFBFBD>ת<EFBFBD><D7AA>
{
//<2F><><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2>
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
g_ptz.vert_dec_mode = PTZ_DEC_DEF;
}
for(k = 0; k <= (PTZ_VERT_DEC_TIME_DEF / 2.0); k++)//K<><4B><EFBFBD><EFBFBD>1<EFBFBD><31>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>2ms
{
if(g_ptz.vert_speed_actual <= PTZ_VERT_BREAK_SPEED * 1.1)
{
break;
}
#ifdef PTZ_PHOTOELECTRIC_SWITCH
if(g_ptz.vert_ps_sw1_state == PS_COVER)//<2F>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
{
break;
}
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
{
break;
}
}
#endif
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
ptz_vert_stop(PTZ_VERT_STOP_TIME);
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
g_ptz.vert_tmc2160 = data;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
g_ptz.vert_tmc2160 = data;
}
}
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
}
}
else//û<><C3BB>ת<EFBFBD><D7AA>
{
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
g_ptz.vert_tmc2160 = data;
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
data = ptz_vert_choice_microstep(PTZ_VERT_BREAK_SPEED);
g_ptz.vert_tmc2160 = data;
}
}
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
}
}
else//g_ptz.vert_rotate_plan.rotate_switch == 0
{
//ɲ<><C9B2><EFBFBD>ر<EFBFBD>ת<EFBFBD><D7AA>
g_ptz.vert_arrive_flag = 0;//ת<><D7AA>ָ<EFBFBD><D6B8>λ<EFBFBD><CEBB>
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
g_ptz.vert_rotate_monitor_switch = 0;
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
}
break;
case PTZ_VERT_REPEAT_LOCATE_C:
BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
/*<2A><>λ*/
if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
{
g_ptz.vert_arrive_flag = 0;//Ԥ<><D4A4>λɨ<CEBB>裬ת<E8A3AC><D7AA>ָ<EFBFBD><D6B8>Ԥ<EFBFBD><D4A4>λ
ptz_location_return_return(LOCATION_VERT);//<2F><>λ<EFBFBD>ش<EFBFBD>
ptz_preset_bit_location_return_return();//Ԥ<><D4A4>λ<EFBFBD><CEBB>λ<EFBFBD>ش<EFBFBD>
g_ptz.vert_rotate_monitor_switch = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ɲ<EFBFBD><C9B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>С<EFBFBD><D0A1>һ<EFBFBD><D2BB><EFBFBD>̶ȣ<CCB6><C8A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Сת<D0A1><D7AA>ת<EFBFBD><D7AA>
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
PTZ_VERT_BREAK_SPEED_ANGLE)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_SPEED;
}
}
break;
}
//<2F><><EFBFBD><EFBFBD>̨<EFBFBD><CCA8>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڼ<EFBFBD><DABC><EFBFBD>λ<EFBFBD><CEBB>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD>Խ<EFBFBD><D4BD>
#ifdef PTZ_PHOTOELECTRIC_SWITCH
if(g_ptz.vert_ps_sw1_state == PS_COVER)//<2F>¸<EFBFBD><C2B8><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_start_stop_set == PTZ_VERT_START
&& g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
{
g_ptz.vert_arrive_flag = 0;
g_ptz.vert_rotate_monitor_switch = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>λ<EFBFBD><CEBB>
{
if(g_ptz.vert_start_stop_set == PTZ_VERT_START
&& g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
{
g_ptz.vert_arrive_flag = 0;
g_ptz.vert_rotate_monitor_switch = 0;
ptz_vert_stop(PTZ_VERT_STOP_TIME);
}
}
#endif
return 1;
}
#endif
static void ptz_hori_rotate_task()
{
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
}
}
static void ptz_vert_rotate_task()
{
while(1)
{
ptz_vert_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 1u);
}
}
static OS_STK task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE];
static void creat_task_hori_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_hori_rotate_task,
(void *) 0,
(OS_STK *)&task_hori_rotate_stk[TASK_HORI_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_HORI_ROATE_MONITOR_PRIO,
(INT16U ) TASK_HORI_ROATE_MONITOR_PRIO,
(OS_STK *)&task_hori_rotate_stk[0],
(INT32U ) TASK_HORI_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_HORI_ROATE_MONITOR_PRIO, "ptz_hori_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_hori_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_hori_rotate_task failed...\n\r");
}
}
static OS_STK task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE];
static void creat_task_vert_rotate(void)
{
CPU_INT08U task_err;
CPU_INT08U name_err;
task_err = OSTaskCreateExt((void (*)(void *)) ptz_vert_rotate_task,
(void *) 0,
(OS_STK *)&task_vert_rotate_stk[TASK_VERT_ROATE_MONITOR_STK_SIZE - 1],
(INT8U ) TASK_VERT_ROATE_MONITOR_PRIO,
(INT16U ) TASK_VERT_ROATE_MONITOR_PRIO,
(OS_STK *)&task_vert_rotate_stk[0],
(INT32U ) TASK_VERT_ROATE_MONITOR_STK_SIZE,
(void *) 0,
(INT16U )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
#if (OS_TASK_NAME_EN > 0)
OSTaskNameSet(TASK_VERT_ROATE_MONITOR_PRIO, "ptz_vert_rotate_task", &name_err);
#endif
if ((task_err == OS_ERR_NONE) && (name_err == OS_ERR_NONE)) {
pdebug(DEBUG_LEVEL_INFO,"create ptz_vert_rotate_task success...\n\r");
} else {
pdebug(DEBUG_LEVEL_FATAL,"create ptz_vert_rotate_task failed...\n\r");
}
}
void init_rotate_monitor_module(void)
{
BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");
BSP_OS_SemCreate(&ptz_vert_stop_mutex,1u,"ptz_vert_stop_mutex");
#ifdef TMC2160
tmc2160_init();
#endif
creat_task_hori_rotate();
creat_task_vert_rotate();
}
#endif