1130 lines
40 KiB
C
1130 lines
40 KiB
C
|
||
#include "rotate_servo.h"
|
||
#include "speed_to_servoMotor.h"
|
||
|
||
#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
|
||
|
||
static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
|
||
static BSP_OS_SEM ptz_vert_stop_mutex;
|
||
|
||
static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
|
||
static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时
|
||
|
||
float ptz_vert_break_angle()
|
||
{
|
||
//云台水平轴加速度(°/s²)
|
||
const float a = 1000.0f * 6.0f
|
||
/ ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f))
|
||
/ PTZ_VERT_RATIO;
|
||
|
||
const float tmp = a / 2.0f;
|
||
|
||
//平稳运行角度，以最低转速运行1秒
|
||
const float s1 = PTZ_VERT_BREAK_SPEED * 1;
|
||
|
||
//当前云台水平轴转速(°/s)
|
||
float Vnow = g_ptz.hori_speed_actual * 6.0f;
|
||
|
||
return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1;
|
||
}
|
||
float ptz_hori_break_angle()
|
||
{
|
||
//云台水平轴加速度(°/s²)
|
||
const float a = 1000.0f * 6.0f
|
||
/ ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f))
|
||
/ PTZ_HORI_RATIO;
|
||
|
||
const float tmp = a / 2.0f;
|
||
|
||
//平稳运行角度，以最低转速运行1秒
|
||
const float s1 = PTZ_HORI_BREAK_SPEED * 1;
|
||
|
||
//当前云台水平轴转速(°/s)
|
||
float Vnow = g_ptz.hori_speed_actual * 6.0f;
|
||
|
||
return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1;
|
||
}
|
||
|
||
void ptz_sem_post_stop_mutex()
|
||
{
|
||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||
}
|
||
|
||
/*!
|
||
\brief 设置伺服电机速度
|
||
\param[in] data：储存发送数据的链表
|
||
\param[out] none
|
||
\retval none
|
||
*/
|
||
void set_speed_to_servoMotor(uint8_t motorType, float speed)
|
||
{
|
||
/*水平电机*/
|
||
if ( motorType == horiMotorType )//水平电机
|
||
{
|
||
//转换为电机端的r/min
|
||
speed = speed * PTZ_HORI_RATIO;
|
||
//设置速度，电机的r/min范围-6000~6000
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
}
|
||
/*垂直电机*/
|
||
else
|
||
{
|
||
//转换为电机端的r/min
|
||
speed = speed * PTZ_VERT_RATIO;
|
||
//设置速度，电机的r/min范围-6000~6000
|
||
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
}
|
||
|
||
}
|
||
|
||
void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
|
||
{
|
||
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
|
||
static float speedTemp;
|
||
switch ( direction )
|
||
{
|
||
case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
|
||
speedTemp = speed;
|
||
break;
|
||
case PTZ_HORI_DIR_LEFT:
|
||
speedTemp = -speed;
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
/*
|
||
-------------------------------------add speed change to here--------------------------------------
|
||
*/
|
||
//设定转动速度
|
||
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
|
||
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
|
||
set_speed_to_servoMotor(horiMotorType, speedTemp);
|
||
|
||
g_ptz.hori_speed_set = speedTemp;
|
||
//设定转动方向
|
||
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;
|
||
//启动电机
|
||
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;
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
|
||
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_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;
|
||
|
||
g_ptz.hori_speed_set = 0;
|
||
g_ptz.hori_speed_actual = 0;
|
||
// 设置速度为0
|
||
set_speed_to_servoMotor(horiMotorType, 0);
|
||
if(ptz_hori_stop_count <= 0)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 0u, time);
|
||
ptz_hori_stop_count = 0;
|
||
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
|
||
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
|
||
}
|
||
ptz_hori_stop_count ++;
|
||
//电子稳定
|
||
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
||
ptz_hori_electric_stable_init();
|
||
#endif
|
||
|
||
BSP_OS_SemPost(&ptz_hori_stop_mutex);
|
||
}
|
||
|
||
void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
|
||
{
|
||
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
|
||
static float speedTemp;
|
||
switch ( direction )
|
||
{
|
||
case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
|
||
speedTemp = speed;
|
||
break;
|
||
case PTZ_VERT_DIR_DOWN:
|
||
speedTemp = -speed;
|
||
break;
|
||
case PTZ_VERT_DIR_STOP:
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
/*
|
||
-------------------------------------add speed change to here--------------------------------------
|
||
*/
|
||
//设定转动速度
|
||
set_speed_to_servoMotor(vertMotorType, speedTemp);
|
||
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
|
||
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
|
||
g_ptz.vert_speed_set = speedTemp;
|
||
//设定转动方向
|
||
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;
|
||
//启动电机
|
||
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);
|
||
|
||
set_speed_to_servoMotor(vertMotorType, 0);
|
||
//停止电机
|
||
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;
|
||
|
||
g_ptz.vert_speed_set = 0;
|
||
g_ptz.vert_speed_actual = 0;
|
||
|
||
if(ptz_vert_stop_count <= 0)
|
||
{
|
||
OSTimeDlyHMSM(0u, 0u, 0u, time);
|
||
ptz_vert_stop_count = 0;
|
||
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
|
||
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
|
||
}
|
||
ptz_vert_stop_count ++;
|
||
//电子稳定
|
||
#ifdef PTZ_ELECTRIC_STABLE_L6235D
|
||
ptz_hori_electric_stable_init();
|
||
#endif
|
||
BSP_OS_SemPost(&ptz_vert_stop_mutex);
|
||
}
|
||
|
||
static char ptz_hori_rotate_monitor_task()
|
||
{
|
||
static float BNP_D;//刹车最近点与当前位置的距离
|
||
static float BFP_D;//刹车最远点与当前位置的距离
|
||
static unsigned int k;
|
||
switch(g_ptz.hori_rotate_monitor_switch)
|
||
{
|
||
case 0://关闭
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
break;
|
||
|
||
case PTZ_HORI_RIGHT_KEEP://向右转动，不刹车
|
||
//关闭转动监控
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
//首先判断云台是否在转动
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
//判断转动方向是否相同
|
||
//如果转向相同，则直接修改设置的转速
|
||
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
|
||
{
|
||
if ( g_ptz.hori_speed_set != g_ptz.hori_rotate_plan.max_speed )
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else//如果转向不同
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
|
||
}
|
||
}
|
||
else//没有转动
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_LEFT_KEEP://向左转动，不刹车
|
||
//关闭转动监控
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
//首先判断云台是否在转动
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
//判断转动方向是否相同
|
||
//如果转向相同，则直接修改设置的转速
|
||
if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
|
||
{
|
||
if ( g_ptz.hori_speed_set != g_ptz.hori_rotate_plan.max_speed )
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else//如果转向不同
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
|
||
}
|
||
}
|
||
else//没有转动
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_GO_TO_ANGLE_A://角度定位
|
||
case PTZ_HORI_MIN_DISTANCE:
|
||
case PTZ_HORI_MAX_DISTANCE:
|
||
case PTZ_HORI_RIGHT_ANGLE:
|
||
case PTZ_HORI_LEFT_ANGLE:
|
||
//首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
|
||
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
|
||
//如果不是处于同一个位置
|
||
//判断是否转动
|
||
//处于转动
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
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//没有转动
|
||
{
|
||
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);
|
||
}
|
||
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
//不需要转动则刹车停止，关闭监控
|
||
g_ptz.hori_arrive_flag = 0;//转动到指定位置
|
||
ptz_location_return_return(LOCATION_HORI);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
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)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
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//没有转动
|
||
{
|
||
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;
|
||
}
|
||
/*定位*/
|
||
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;
|
||
}
|
||
/*定位*/
|
||
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
|
||
{
|
||
if(g_ptz.hori_repeat_locate_switch == 0)//重复定位关闭
|
||
{
|
||
g_ptz.hori_arrive_flag = 0;//转到指定位置
|
||
ptz_location_return_return(LOCATION_HORI);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
}
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
if(g_ptz.hori_repeat_locate_switch == 1)//重复定位打开
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A;
|
||
}
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_BRAKE://停止转动
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
break;
|
||
|
||
case PTZ_HORI_DEC_BRAKE_A://减速刹车
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B;
|
||
k = 0;
|
||
break;
|
||
|
||
case PTZ_HORI_DEC_BRAKE_B:
|
||
k++;//K增加1，时间增加1ms
|
||
if(g_ptz.hori_speed_actual <= PTZ_HORI_MIN_SPEED * 2 ||
|
||
k >= 500)
|
||
{
|
||
k = 0;
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
}
|
||
|
||
break;
|
||
|
||
//重复定位路径规划
|
||
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;
|
||
//重复定位启动控制
|
||
case PTZ_HORI_REPEAT_LOCATE_B:
|
||
//首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
|
||
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C;
|
||
//如果不是处于同一个位置
|
||
//判断是否转动
|
||
//处于转动
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
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//没有转动
|
||
{
|
||
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);
|
||
}
|
||
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
//不需要转动则刹车停止，关闭监控
|
||
g_ptz.hori_arrive_flag = 0;//转到指定预位置
|
||
ptz_location_return_return(LOCATION_HORI);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
}
|
||
}
|
||
break;
|
||
//重复定位判断是否达到定位位置
|
||
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;
|
||
}
|
||
/*定位*/
|
||
if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
|
||
{
|
||
g_ptz.hori_arrive_flag = 0;//转到指定位置
|
||
ptz_location_return_return(LOCATION_HORI);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case PTZ_HORI_RIGHT_ANGLE_INC:
|
||
//首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
|
||
if(g_ptz.hori_rotate_plan.rotate_switch == 1)
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
|
||
//如果不是处于同一个位置
|
||
//判断是否转动
|
||
//处于转动
|
||
if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
|
||
{
|
||
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_hori_stop(PTZ_HORI_STOP_TIME);
|
||
//再启动
|
||
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//没有转动
|
||
{
|
||
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);
|
||
}
|
||
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
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)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
|
||
PTZ_HORI_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
}
|
||
else//g_ptz.hori_rotate_plan.rotate_switch == 0;
|
||
{
|
||
g_ptz.hori_rotate_monitor_switch = 0;
|
||
//不需要转动则刹车停止，关闭监控
|
||
g_ptz.hori_arrive_flag = 0;//转动到指定位置
|
||
ptz_location_return_return(LOCATION_HORI);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
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;
|
||
}
|
||
// if ( last_hori_rotate_monitor_swicth != now_hori_rotate_monitor_swicth )
|
||
// {
|
||
// set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
|
||
// }
|
||
return 1;
|
||
}
|
||
|
||
static char ptz_vert_rotate_monitor_task()
|
||
{
|
||
static float BNP_D;//刹车最近点与当前位置的距离
|
||
static float BFP_D;//刹车最远点与当前位置的距离
|
||
static char cmd_type;
|
||
static unsigned int k;
|
||
switch(g_ptz.vert_rotate_monitor_switch)
|
||
{
|
||
case 0://关闭
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
|
||
break;
|
||
|
||
case PTZ_VERT_GO_TO_ANGLE_A://转动到指定角度启动
|
||
case PTZ_VERT_ANGLE:
|
||
case PTZ_VERT_UP_KEEP:
|
||
case PTZ_VERT_DOWN_KEEP:
|
||
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)
|
||
{
|
||
//首先判断是否转动
|
||
//处于转动
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
|
||
{
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
//再启动
|
||
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
||
}
|
||
}
|
||
else//没有转动
|
||
{
|
||
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
||
PTZ_VERT_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
|
||
}
|
||
else//g_ptz.vert_rotate_plan.rotate_switch == 0
|
||
{
|
||
//刹车关闭转动
|
||
g_ptz.vert_arrive_flag = 0;//转到指定预位置
|
||
ptz_location_return_return(LOCATION_VERT);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
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://转动到指定角度定位
|
||
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);
|
||
/*定位*/
|
||
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;//转到指定预位置
|
||
ptz_location_return_return(LOCATION_VERT);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
}
|
||
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);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
||
PTZ_VERT_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
break;
|
||
|
||
case PTZ_VERT_BRAKE://刹车
|
||
g_ptz.vert_rotate_monitor_switch = 0;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
break;
|
||
|
||
case PTZ_VERT_DEC_BRAKE_A://垂直减速刹车
|
||
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B;
|
||
k = 0;
|
||
break;
|
||
|
||
case PTZ_VERT_DEC_BRAKE_B:
|
||
k ++;//K增加1,时间增加1ms
|
||
if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 2 ||
|
||
k >= 500)
|
||
{
|
||
k = 0;
|
||
g_ptz.vert_rotate_monitor_switch = 0;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
}
|
||
break;
|
||
|
||
//重复定位路径规划
|
||
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;
|
||
//重复定位转动启动
|
||
case PTZ_VERT_REPEAT_LOCATE_B:
|
||
if(g_ptz.vert_rotate_plan.rotate_switch == 1)
|
||
{
|
||
g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C;
|
||
//首先判断是否转动
|
||
//处于转动
|
||
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
|
||
{
|
||
//判断转向是否相同
|
||
//如果转向相同
|
||
if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
|
||
{
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
else//转向不同，则直接修改设置的转速
|
||
{
|
||
//则先刹车
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
//再启动
|
||
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
||
}
|
||
}
|
||
else//没有转动
|
||
{
|
||
g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
|
||
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
|
||
ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
||
PTZ_VERT_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
|
||
}
|
||
else//g_ptz.vert_rotate_plan.rotate_switch == 0
|
||
{
|
||
//刹车关闭转动
|
||
g_ptz.vert_arrive_flag = 0;//转到指定位置
|
||
ptz_location_return_return(LOCATION_VERT);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
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);
|
||
/*定位*/
|
||
if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
|
||
{
|
||
g_ptz.vert_arrive_flag = 0;//预置位扫描，转到指定预置位
|
||
ptz_location_return_return(LOCATION_VERT);//定位回传
|
||
ptz_preset_bit_location_return_return();//预置位到位回传
|
||
g_ptz.vert_rotate_monitor_switch = 0;
|
||
ptz_vert_stop(PTZ_VERT_STOP_TIME);
|
||
}
|
||
//如果离最近刹车距离小于一定程度，则以最小转速转动
|
||
if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
|
||
PTZ_VERT_BREAK_SPEED_ANGLE)
|
||
{
|
||
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
|
||
// set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
|
||
}
|
||
break;
|
||
}
|
||
|
||
//当云台垂直方向处于极限位置时，防止垂直方向角度越界
|
||
#ifdef PTZ_PHOTOELECTRIC_SWITCH
|
||
if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置
|
||
{
|
||
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);//停止延时为0
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
}
|
||
if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
|
||
{
|
||
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);
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
|
||
}
|
||
}
|
||
#endif
|
||
|
||
return 1;
|
||
}
|
||
|
||
|
||
static void ptz_hori_rotate_task()
|
||
{
|
||
while(1)
|
||
{
|
||
ptz_hori_rotate_monitor_task();
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
}
|
||
}
|
||
|
||
|
||
static void ptz_vert_rotate_task()
|
||
{
|
||
while(1)
|
||
{
|
||
ptz_vert_rotate_monitor_task();
|
||
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
|
||
}
|
||
}
|
||
|
||
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");
|
||
|
||
creat_task_hori_rotate();
|
||
creat_task_vert_rotate();
|
||
}
|
||
|
||
|
||
#endif |