diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c index 92c58b9..596e01f 100644 --- a/APP/Device/Device_rotate/rotate_servo.c +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -59,11 +59,12 @@ void ptz_sem_post_stop_mutex() */ void set_speed_to_servoMotor(uint8_t motorType, float speed) { - //转换为电机端的r/min - speed = speed * PTZ_HORI_RATIO + 0.5; + /*水平电机*/ if ( motorType == horiMotorType )//水平电机 { + //转换为电机端的r/min + speed = speed * PTZ_HORI_RATIO; //设置速度，电机的r/min范围-6000~6000 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed) , WRITE_ONE_REG_FRAME_NUM, lowPriority); @@ -71,6 +72,8 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed) /*垂直电机*/ else { + //转换为电机端的r/min + speed = speed * PTZ_VERT_RATIO; //设置速度，电机的r/min范围-6000~6000 servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed) , WRITE_ONE_REG_FRAME_NUM, lowPriority); @@ -139,10 +142,9 @@ 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); //使能电机 - OSTimeDlyHMSM(0u, 0u, 0u, 50u); g_ptz.hori_speed_set = speedTemp; //设定转动方向 g_ptz.hori_direction_last = g_ptz.hori_direction_set; @@ -161,15 +163,13 @@ void ptz_hori_stop(unsigned short int time) BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u); set_speed_to_servoMotor(horiMotorType, 0); -// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0) -// , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 + //停止电机 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; -// set_speed_to_servoMotor(horiMotorType, 0); g_ptz.hori_speed_set = 0; g_ptz.hori_speed_actual = 0; @@ -181,8 +181,6 @@ void ptz_hori_stop(unsigned short int time) , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 } ptz_hori_stop_count ++; -// servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0) -// , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 //电子稳定 #ifdef PTZ_ELECTRIC_STABLE_L6235D ptz_hori_electric_stable_init(); @@ -198,10 +196,12 @@ void ptz_vert_start(char direction, float speed)//输入参数的speed是云台 switch ( direction ) { case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上 + break; case PTZ_VERT_DIR_DOWN: + speed = -speed; break; case PTZ_VERT_DIR_STOP: - speed = -speed; + break; default: break; } @@ -230,17 +230,13 @@ void ptz_vert_stop(unsigned short int time) { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); + set_speed_to_servoMotor(vertMotorType, 0); //停止电机 g_ptz.vert_start_stop_set = PTZ_VERT_STOP; g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set; g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP; - - //设定转动速度 - // servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度 - // , WRITE_ONE_REG_FRAME_NUM, lowPriority); - set_speed_to_servoMotor(vertMotorType, 0); g_ptz.vert_speed_set = 0; g_ptz.vert_speed_actual = 0; @@ -248,9 +244,9 @@ void ptz_vert_stop(unsigned short int time) { OSTimeDlyHMSM(0u, 0u, 0u, time); ptz_vert_stop_count = 0; + servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0) + , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 } - servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0) - , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 ptz_vert_stop_count ++; //电子稳定 #ifdef PTZ_ELECTRIC_STABLE_L6235D diff --git a/APP/Device/Device_speed/servoMotor_recv.c b/APP/Device/Device_speed/servoMotor_recv.c index 1f72e21..bd2e8e3 100644 --- a/APP/Device/Device_speed/servoMotor_recv.c +++ b/APP/Device/Device_speed/servoMotor_recv.c @@ -251,15 +251,10 @@ static void ptz_recv_hori_servo_task() CPU_INT08U err; while(1) { OSSemPend(g_horiMotorMutex, 0, &err); -// stopTimeOut(H_MOTOR); if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false) { -// servoLinkListMemPut(horiMotorType); - // H_MOTOR_STOP; -// stopTimeOut(H_MOTOR); continue; } -// OSTimeDlyHMSM(0u, 0u, 0u, 5u); stopTimeOut(H_MOTOR); //释放信号量，通知能发送一次 OSSemPost(g_horiSpeedMutex); @@ -277,13 +272,11 @@ static void ptz_recv_vert_servo_task() CPU_INT08U err; while(1) { OSSemPend(g_vertMotorMutex, 0, &err); - stopTimeOut(V_MOTOR); if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false) { - // V_MOTOR_STOP; continue; } - + stopTimeOut(V_MOTOR); //释放信号量，通知能发送一次 OSSemPost(g_vertSpeedMutex); } diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index 3c76ce1..db834b0 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -310,6 +310,7 @@ static void ptz_hori_servo_task() else { pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task no data...\n\r"); } + OSTimeDlyHMSM(0u, 0u, 0u, 10u); } } @@ -346,7 +347,7 @@ static void ptz_vert_servo_task() else { pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task no data...\n\r"); } - + OSTimeDlyHMSM(0u, 0u, 0u, 10u); } } diff --git a/APP/Service/service_selfcheck.c b/APP/Service/service_selfcheck.c index 59a2e82..ee7e534 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;