MW22-02A/APP/Device/Device_rotate/rotate_bldc.c

1011 lines
32 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

#include "gd32f4xx.h"
#include "ptz_header_file.h"
#include "math.h"
#include "ptz_default_value.h"
#include "ptz_type_select.h"
#include "l6235d.h"
#include "service_presetbitscan.h"
#ifdef PTZ_BLDC_MOTOR
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;//垂直停止计数,防止多次停止多次延时
void ptz_sem_post_stop_mutex()//释放云台启停共享资源锁
{
BSP_OS_SemPost(&ptz_hori_stop_mutex);
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_hori_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
ptz_hori_pid_clear_zero();
//设定转动速度
g_ptz.hori_speed_set = speed;
//设定转动方向
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;
#ifdef PTZ_MOTOR_CONTROL_L6235D
l6235d_hori_set_direction(direction);
l6235d_hori_start_stop(PTZ_HORI_START);
#endif
ptz_hori_stop_count = 0;
#ifdef PTZ_ELECTRIC_STABLE_L6235D
g_ptz.hori_electric_stable.stable_stop_switch = 0;
#endif
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;
#ifdef PTZ_MOTOR_CONTROL_L6235D
l6235d_hori_start_stop(PTZ_HORI_STOP);
#endif
//PID清0
ptz_hori_pid_clear_zero();
//设定转动速度
g_ptz.hori_speed_set = 0;
g_ptz.hori_speed_actual = 0;
if(ptz_hori_stop_count <= 0)
{
OSTimeDlyHMSM(0u, 0u, 0u, time);
ptz_hori_stop_count = 0;
}
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)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
ptz_vert_pid_clear_zero();
//设定转动速度
g_ptz.vert_speed_set = speed;
//设定转动方向
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;
#ifdef PTZ_MOTOR_CONTROL_L6235D
l6235d_vert_set_direction(direction);
l6235d_vert_start_stop(PTZ_VERT_START);
#endif
ptz_vert_stop_count = 0;
#ifdef PTZ_ELECTRIC_STABLE_L6235D
g_ptz.vert_electric_stable.stable_stop_switch = 0;
#endif
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
void ptz_vert_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
{
//停止电机
g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
#ifdef PTZ_MOTOR_CONTROL_L6235D
l6235d_vert_start_stop(PTZ_VERT_STOP);
#endif
//PID清0
ptz_vert_pid_clear_zero();
//设定转动速度
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;
}
ptz_vert_stop_count ++;
//云台电子稳定
#ifdef PTZ_ELECTRIC_STABLE_L6235D
ptz_vert_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://关闭
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)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
}
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)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
}
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
}
}
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
}
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;
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 * 1.5 ||
k >= 1400)
{
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
}
}
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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
}
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_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_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
else
{
if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <=
PTZ_HORI_MIN_SPEED_ANGLE)
{
g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
}
}
}
}
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;
}
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://关闭
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
}
}
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_MIN_SPEED_ANGLE)
{
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
}
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 * 1.5 ||
k >= 1400)
{
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;
}
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_MIN_SPEED_ANGLE)
{
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
}
}
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_MIN_SPEED_ANGLE)
{
g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
}
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);
}
}
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);
}
}
#endif
return 1;
}
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
}
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
}
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");
l6235d_init();
creat_task_hori_rotate();
creat_task_vert_rotate();
}
#endif