From 4b086aae75952886383dce98cd9e9dcbd2f5ed4c Mon Sep 17 00:00:00 2001 From: "REASEARCHER\\18383" <1633026436@qq.com> Date: Mon, 20 Oct 2025 16:28:44 +0800 Subject: [PATCH] =?UTF-8?q?=E9=93=BE=E8=A1=A8=E9=87=8A=E6=94=BE=E8=BF=98?= =?UTF-8?q?=E6=98=AF=E6=9C=89=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- APP/Device/Device_rotate/rotate_servo.c | 900 ++++++++++++++++-- APP/Device/Device_speed/speed_to_servoMotor.c | 30 +- APP/Service/service_selfcheck.c | 4 +- BSP/Driver/servoMotor/motorCommu.c | 8 +- 4 files changed, 846 insertions(+), 96 deletions(-) diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c index 596e01f..37d7b43 100644 --- a/APP/Device/Device_rotate/rotate_servo.c +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -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); } } diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index db834b0..757ce1b 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -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); } diff --git a/APP/Service/service_selfcheck.c b/APP/Service/service_selfcheck.c index ee7e534..7bd86c4 100644 --- a/APP/Service/service_selfcheck.c +++ b/APP/Service/service_selfcheck.c @@ -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++; } diff --git a/BSP/Driver/servoMotor/motorCommu.c b/BSP/Driver/servoMotor/motorCommu.c index b23881d..f63ca75 100644 --- a/BSP/Driver/servoMotor/motorCommu.c +++ b/BSP/Driver/servoMotor/motorCommu.c @@ -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);