pengsx
/
servoMotor
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

链表释放还是有问题

This commit is contained in:
REASEARCHER\18383 2025-10-20 16:28:44 +08:00
parent b84ffa3e9b
commit 4b086aae75
4 changed files with 846 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

900
APP/Device/Device_rotate/rotate_servo.c
View File

@ -59,7 +59,6 @@ void ptz_sem_post_stop_mutex()
*/
void set_speed_to_servoMotor(uint8_t motorType, float speed)
{
/*水平电机*/
if ( motorType == horiMotorType )//水平电机
{
@ -78,48 +77,9 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed)
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
}
// //转换为电机端的r/min
// static float speedTemp;
// speed = speed * PTZ_HORI_RATIO + 0.5;
// speedTemp = speed;
// speedTemp < 0 ? speedTemp *= -1 : speedTemp; //变为正数
//
// if (motorType == horiMotorType) {
// if (speedTemp > PTZ_HORI_MOTOR_MAX_SPEED) {
// speedTemp = PTZ_HORI_MOTOR_MAX_SPEED;
// }
// else if (speedTemp < PTZ_HORI_MIN_SPEED) {
// speedTemp = PTZ_HORI_MOTOR_MIN_SPEED;
// }
// //设置速度，电机的r/min范围-6000~6000
// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// }
// else if (motorType == vertMotorType) {
// if (speedTemp > PTZ_VERT_MOTOR_MAX_SPEED) {
// speedTemp = PTZ_VERT_MOTOR_MAX_SPEED;
// }
// else if (speedTemp < PTZ_VERT_MOTOR_MIN_SPEED) {
// speedTemp = PTZ_VERT_MOTOR_MIN_SPEED;
// }
//
// //设置速度，电机的r/min范围-6000~6000
// servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);
// }
}
/*
///云台水平右转
#define PTZ_HORI_DIR_RIGHT 1
///云台水平左转
#define PTZ_HORI_DIR_LEFT 3//0
///云台处于停止状态
#define PTZ_HORI_DIR_STOP 2
*/
void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
@ -142,9 +102,7 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
// 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);
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speedTemp;
//设定转动方向
g_ptz.hori_direction_last = g_ptz.hori_direction_set;
@ -155,6 +113,8 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
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);
}
@ -162,8 +122,6 @@ void ptz_hori_stop(unsigned short int time)
{
BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
set_speed_to_servoMotor(horiMotorType, 0);
//停止电机
g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
@ -172,13 +130,14 @@ void ptz_hori_stop(unsigned short int time)
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, lowPriority); //失能电机
, WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机
}
ptz_hori_stop_count ++;
//电子稳定
@ -192,13 +151,14 @@ void ptz_hori_stop(unsigned short int time)
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:
speed = -speed;
speedTemp = -speed;
break;
case PTZ_VERT_DIR_STOP:
break;
@ -209,17 +169,17 @@ void ptz_vert_start(char direction, float speed)//输入参数的speed是云台
-------------------------------------add speed change to here--------------------------------------
*/
//设定转动速度
set_speed_to_servoMotor(vertMotorType, speed);
servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
set_speed_to_servoMotor(vertMotorType, speedTemp);
servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
, WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
g_ptz.hori_speed_set = speed;
g_ptz.vert_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.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.hori_start_stop_set = PTZ_HORI_START;
g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_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;
@ -255,14 +215,830 @@ void ptz_vert_stop(unsigned short int time)
BSP_OS_SemPost(&ptz_vert_stop_mutex);
}
static void ptz_hori_rotate_monitor_task()
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 void ptz_vert_rotate_monitor_task()
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;
}
@ -271,7 +1047,7 @@ static void ptz_hori_rotate_task()
while(1)
{
ptz_hori_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
}
@ -281,7 +1057,7 @@ static void ptz_vert_rotate_task()
while(1)
{
ptz_vert_rotate_monitor_task();
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
OSTimeDlyHMSM(0u, 0u, 0u, 2u);
}
}

30
APP/Device/Device_speed/speed_to_servoMotor.c
View File

@ -297,7 +297,6 @@ static void ptz_hori_servo_task()
startTimeOut(horiMotorType);
continue;
}
// 高优先级链表中无数据时，发送低优先级中的数据
if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
//发送数据
@ -550,39 +549,12 @@ void init_speed_module(void)
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0),
WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_MOTOR_DIR_SELEC, PTZ_HORI_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置水平电机方向
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_MOTOR_DIR_SELEC, PTZ_VERT_MOTOR_DIRECTION)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置垂直电机方向
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_HORI_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_VERT_MOTOR_AccelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_HORI_MOTOR_DecelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_VERT_MOTOR_DecelerationTimeConstant)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
// servoSendData(i, WriteMotorOneReg(i, H06_SPEED_COMMU_SET_VALUE, 60)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
// OSTimeDlyHMSM(0u, 0u, 0u, 5u);
servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
, WRITE_ONE_REG_FRAME_NUM, lowPriority);
// servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
// servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
// , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
OSTimeDlyHMSM(0u, 0u, 0u, 5u);
}

4
APP/Service/service_selfcheck.c
View File

@ -956,7 +956,7 @@ static unsigned char ptz_vert_complete_self_check_task()
{
///首先读取光电开关的状态，初步判断垂直状态
case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
// set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_ps_sw1_down_fall = 0;
g_ptz.vert_ps_sw1_up_rise = 0;
g_ptz.vert_ps_sw2_up_fall = 0;
@ -1020,6 +1020,7 @@ static unsigned char ptz_vert_complete_self_check_task()
g_ptz.vert_ps_sw1_up_rise = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check++;
}
@ -1148,6 +1149,7 @@ static unsigned char ptz_vert_simplify_self_check_task()
g_ptz.vert_ps_sw2_down_rise = 0;
OSTimeDlyHMSM(0u, 0u, 0u, 300u);
ptz_vert_stop(PTZ_VERT_STOP_TIME);
OSTimeDlyHMSM(0u, 0u, 0u, 10u);
ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
g_ptz.vert_self_check++;
}

8
BSP/Driver/servoMotor/motorCommu.c
View File

@ -210,11 +210,11 @@ static void DmaCofig(void)
//dma中断配置
if( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 1, 2);
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 2, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 1, 1);
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 2, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, DMA_CHXCTL_FTFIE);//dma发送完成中断
@ -238,11 +238,11 @@ static void DmaCofig(void)
//中断配置
if ( i == H_MOTOR )
{
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 0, 2);
nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 1, 2);
}
else
{
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 0, 1);
nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 1, 1);
}
dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE);
dma_channel_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);