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

1814 lines
58 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 "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;//共享资源锁
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);
}
//转速r/min转化为步进电机步进频率,step 细分程度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;
}
//步进电机步进频率转化为转速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;
}
//转速r/min转化为步进电机步进频率,step 细分程度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;
}
//步进电机步进频率转化为转速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;
}
//根据转速判断采用的细分程度,输入速度v是云台的转速
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;
//给一个预设的默认值
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;
//给一个预设的默认值
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;
//设定转动速度
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;
tmc2160_set_hori_dir(direction);
//驱动芯片寄存器写数据
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;
//设定转动速度
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;
//停止电机
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//是否打开电子自锁
// 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);
}
/// 云台水平转动监控
static char ptz_hori_rotate_monitor_task()
{
static float BNP_D;//刹车最近点与当前位置的距离
static float BFP_D;//刹车最远点与当前位置的距离
static unsigned int k;
TMC2160Control data;
switch(g_ptz.hori_rotate_monitor_switch)
{
case 0://关闭
break;
case PTZ_HORI_RIGHT_KEEP://向右转动,不刹车
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
//关闭转动监控
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_tmc2160.actual_microstep == data.microstep)
// {
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同,直接赋值速度
// }else{
//
// g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//先改细分再赋值速度,,,解决转动过程中改变速度引起的停顿
// 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{
//如果转向不同
//则先刹车
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增加1时间增加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);
//再启动
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);
}
// //判断转动方向是否相同
// //如果转向相同且细分程度相同,则直接修改设置的转速
// 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//如果转向不同
// {
// //则先刹车
//
// 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增加1时间增加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);
// //再启动
// 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//没有转动
{
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://向左转动,不刹车
data = ptz_hori_choice_microstep(g_ptz.hori_rotate_plan.max_speed);
//关闭转动监控
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_tmc2160.actual_microstep == data.microstep)
// {
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同,直接赋值速度
// }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;//先改细分再赋值速度
// g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
// }
}
else//如果转向不同
{
//则先刹车
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增加1时间增加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);
//再启动
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);
}
// //判断转动方向是否相同
// //如果转向相同且细分程度相同,则直接修改设置的转速
// 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//如果转向不同
// {
// //则先刹车
// 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增加1时间增加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);
// //再启动
// 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//没有转动
{
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://角度定位
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);
//首先判断是否需要转动,如果转动距离为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)
{
// if(g_ptz.hori_tmc2160.actual_microstep == data.microstep)
// {//如果转向相同且细分程度相同
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;//细分相同,直接赋值速度
// }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;//先改细分再赋值速度
// g_ptz.hori_tmc2160.actual_microstep = g_ptz.hori_tmc2160.microstep;
// }
}
else//转向不同,则直接修改设置的转速
{
//则先刹车
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增加1时间增加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);
//再启动
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//没有转动
{
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);
}
//如果离最近刹车距离小于一定程度,则以最小转速转动
//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;
//不需要转动则刹车停止,关闭监控
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);
}
//如果离最近刹车距离小于一定程度,则以最小转速转动
//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://停止转动
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_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增加1时间增加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;
//重复定位路径规划
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即同一个位置则不需要转动
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;
//如果不是处于同一个位置
//判断是否转动
//处于转动
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_tmc2160.actual_microstep == data.microstep)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
}
else//转向不同,则直接修改设置的转速
{
//则先刹车
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增加1时间增加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);
//再启动
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//没有转动
{
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);
}
//如果离最近刹车距离小于一定程度,则以最小转速转动
//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;
//不需要转动则刹车停止,关闭监控
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);
}
//如果离最近刹车距离小于一定程度,则以最小转速转动
//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);
//首先判断是否需要转动,如果转动距离为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_tmc2160.actual_microstep == data.microstep)
{
g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
// g_ptz.hori_tmc2160 = data;
}
else//转向不同,则直接修改设置的转速
{
//则先刹车
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增加1时间增加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);
//再启动
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//没有转动
{
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);
//如果离最近刹车距离小于一定程度,则以最小转速转动
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;
//不需要转动则刹车停止,关闭监控
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;
}
void ptz_vert_start(char direction, float speed)
{
BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
g_ptz.vert_dec_mode = PTZ_DEC_MAX;
//设定转动速度
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;
tmc2160_set_vert_dir(direction);
//写寄存器数据
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)
{
//设定转动速度
g_ptz.vert_speed_set = 0;
g_ptz.vert_speed_actual = 0;
//若需要自锁,则驱动器要保持使能状态
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;
//停止电机
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//是否打开电子自锁
// 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);
}
/// 云台垂直转动监控
static char ptz_vert_rotate_monitor_task()
{
static float BNP_D;//刹车最近点与当前位置的距离
static float BFP_D;//刹车最远点与当前位置的距离
static char cmd_type;
static unsigned int k;
TMC2160Control data;
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:
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)
{
//首先判断是否转动
//处于转动
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;
// if(g_ptz.vert_tmc2160.actual_microstep == data.microstep)
// {//如果转向相同且细分程度相同
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;//细分相同,直接赋值速度
// }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;//先改细分再赋值速度
// g_ptz.vert_tmc2160.actual_microstep = g_ptz.vert_tmc2160.microstep;
// }
}
else//转向不同
{
//则先刹车
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增加1时间增加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)//下俯极限位置
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
{
break;
}
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
{
break;
}
}
#endif
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
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;
g_ptz.vert_tmc2160 = data;
//如果离最近刹车距离小于一定程度,则以最小转速转动
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_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;
//如果离最近刹车距离小于一定程度,则以最小转速转动
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
{
//刹车关闭转动
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);
}
//如果离最近刹车距离小于一定程度,则以最小转速转动
//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://刹车
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_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增加1,时间增加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;
//重复定位路径规划
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:
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;
//首先判断是否转动
//处于转动
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_tmc2160.actual_microstep == data.microstep)
{
g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
}
else//转向不同,则直接修改设置的转速
{
//则先刹车
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增加1时间增加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)//下俯极限位置
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
{
break;
}
}
if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
{
if(g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
{
break;
}
}
#endif
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
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;
g_ptz.vert_tmc2160 = data;
//如果离最近刹车距离小于一定程度,则以最小转速转动
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_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;
//如果离最近刹车距离小于一定程度,则以最小转速转动
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
{
//刹车关闭转动
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)
{
if(g_ptz.vert_speed_set > PTZ_VERT_BREAK_SPEED)
{
g_ptz.vert_speed_set = PTZ_VERT_BREAK_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;
}
#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