#include "rotate_servo.h" #include "speed_to_servoMotor.h" #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V 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;//垂直停止计数，防止多次停止多次延时 float ptz_vert_break_angle() { //云台水平轴加速度(°/s²) const float a = 1000.0f * 6.0f / ((float)(PTZ_VERT_MOTOR_DecelerationTimeConstant / 1000.0f)) / PTZ_VERT_RATIO; const float tmp = a / 2.0f; //平稳运行角度，以最低转速运行1秒 const float s1 = PTZ_VERT_BREAK_SPEED * 1; //当前云台水平轴转速(°/s) float Vnow = g_ptz.hori_speed_actual * 6.0f; return (Vnow - PTZ_VERT_BREAK_SPEED) * (Vnow + PTZ_VERT_BREAK_SPEED) / tmp + s1; } float ptz_hori_break_angle() { //云台水平轴加速度(°/s²) const float a = 1000.0f * 6.0f / ((float)(PTZ_HORI_MOTOR_DecelerationTimeConstant / 1000.0f)) / PTZ_HORI_RATIO; const float tmp = a / 2.0f; //平稳运行角度，以最低转速运行1秒 const float s1 = PTZ_HORI_BREAK_SPEED * 1; //当前云台水平轴转速(°/s) float Vnow = g_ptz.hori_speed_actual * 6.0f; return (Vnow - PTZ_HORI_BREAK_SPEED) * (Vnow + PTZ_HORI_BREAK_SPEED) / tmp + s1; } void ptz_sem_post_stop_mutex() { BSP_OS_SemPost(&ptz_hori_stop_mutex); BSP_OS_SemPost(&ptz_vert_stop_mutex); } /*! \brief 设置伺服电机速度 \param[in] data：储存发送数据的链表 \param[out] none \retval none */ void set_speed_to_servoMotor(uint8_t motorType, float speed) { //转换为电机端的r/min speed = speed * PTZ_HORI_RATIO + 0.5; /*水平电机*/ if ( motorType == horiMotorType )//水平电机 { //设置速度，电机的r/min范围-6000~6000 servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed) , WRITE_ONE_REG_FRAME_NUM, lowPriority); } /*垂直电机*/ else { //设置速度，电机的r/min范围-6000~6000 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); static float speedTemp; switch ( direction ) { case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转 speedTemp = speed; break; case PTZ_HORI_DIR_LEFT: speedTemp = -speed; break; default: break; } /* -------------------------------------add speed change to here-------------------------------------- */ //设定转动速度 // 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); 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; g_ptz.hori_direction_set = direction; g_ptz.hori_direction_actual = g_ptz.hori_direction_set; //启动电机 g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; 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); 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; 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); //失能电机 } 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(); #endif BSP_OS_SemPost(&ptz_hori_stop_mutex); } void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min { BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u); switch ( direction ) { case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上 case PTZ_VERT_DIR_DOWN: break; case PTZ_VERT_DIR_STOP: speed = -speed; default: break; } /* -------------------------------------add speed change to here-------------------------------------- */ //设定转动速度 set_speed_to_servoMotor(vertMotorType, speed); servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机 g_ptz.hori_speed_set = speed; //设定转动方向 g_ptz.hori_direction_last = g_ptz.hori_direction_set; g_ptz.hori_direction_set = direction; g_ptz.hori_direction_actual = g_ptz.hori_direction_set; //启动电机 g_ptz.hori_start_stop_set = PTZ_HORI_START; g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set; 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_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; if(ptz_vert_stop_count <= 0) { 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); //失能电机 ptz_vert_stop_count ++; //电子稳定 #ifdef PTZ_ELECTRIC_STABLE_L6235D ptz_hori_electric_stable_init(); #endif BSP_OS_SemPost(&ptz_vert_stop_mutex); } static void ptz_hori_rotate_monitor_task() { } static void ptz_vert_rotate_monitor_task() { } static void ptz_hori_rotate_task() { while(1) { ptz_hori_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 10u); } } static void ptz_vert_rotate_task() { while(1) { ptz_vert_rotate_monitor_task(); OSTimeDlyHMSM(0u, 0u, 0u, 10u); } } 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"); creat_task_hori_rotate(); creat_task_vert_rotate(); } #endif