diff --git a/APP/Device/Device_rotate/rotate_servo.c b/APP/Device/Device_rotate/rotate_servo.c index a62f2e4..92c58b9 100644 --- a/APP/Device/Device_rotate/rotate_servo.c +++ b/APP/Device/Device_rotate/rotate_servo.c @@ -61,30 +61,51 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed) { //转换为电机端的r/min speed = speed * PTZ_HORI_RATIO + 0.5; - - if (motorType == horiMotorType) { - if (speed > PTZ_HORI_MOTOR_MAX_SPEED) { - speed = PTZ_HORI_MOTOR_MAX_SPEED; - } - else if (speed < PTZ_HORI_MIN_SPEED) { - speed = PTZ_HORI_MOTOR_MIN_SPEED; - } + /*水平电机*/ + if ( motorType == horiMotorType )//水平电机 + { //设置速度,电机的r/min范围-6000~6000 - servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed) + 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); } - else if (motorType == vertMotorType) { - if (speed > PTZ_VERT_MOTOR_MAX_SPEED) { - speed = PTZ_VERT_MOTOR_MAX_SPEED; - } - else if (speed < PTZ_VERT_MOTOR_MIN_SPEED) { - speed = PTZ_VERT_MOTOR_MIN_SPEED; - } + - //设置速度,电机的r/min范围-6000~6000 - servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_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); +// } } @@ -99,13 +120,15 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed) 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://伺服电机默认速度为正使云台右转 - case PTZ_HORI_DIR_STOP: - break; + speedTemp = speed; + break; case PTZ_HORI_DIR_LEFT: - speed = -speed; + speedTemp = -speed; + break; default: break; } @@ -115,10 +138,12 @@ 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, speed); + set_speed_to_servoMotor(horiMotorType, speedTemp); + servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1) , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机 - g_ptz.hori_speed_set = speed; + 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; @@ -135,17 +160,16 @@ 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; - - //设定转动速度 - // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度 - // , WRITE_ONE_REG_FRAME_NUM, lowPriority); - set_speed_to_servoMotor(horiMotorType, 0); +// set_speed_to_servoMotor(horiMotorType, 0); g_ptz.hori_speed_set = 0; g_ptz.hori_speed_actual = 0; @@ -153,10 +177,12 @@ void ptz_hori_stop(unsigned short int time) { 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); //失能电机 +// 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(); diff --git a/APP/Device/Device_speed/servoMotor_recv.c b/APP/Device/Device_speed/servoMotor_recv.c index dbe9ca1..1f72e21 100644 --- a/APP/Device/Device_speed/servoMotor_recv.c +++ b/APP/Device/Device_speed/servoMotor_recv.c @@ -251,13 +251,16 @@ static void ptz_recv_hori_servo_task() CPU_INT08U err; while(1) { OSSemPend(g_horiMotorMutex, 0, &err); - stopTimeOut(H_MOTOR); +// 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); } diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c index 92c6e0d..3c76ce1 100644 --- a/APP/Device/Device_speed/speed_to_servoMotor.c +++ b/APP/Device/Device_speed/speed_to_servoMotor.c @@ -546,7 +546,6 @@ void init_speed_module(void) servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_CONTR_MODE_SELEC, 0), WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式 -// OSTimeDlyHMSM(0u, 0u, 0u, 5u); servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0), WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式 OSTimeDlyHMSM(0u, 0u, 0u, 5u); @@ -573,10 +572,16 @@ void init_speed_module(void) // , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min // OSTimeDlyHMSM(0u, 0u, 0u, 5u); - 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);//电机运行使能 + 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_error_count.c b/APP/Service/service_error_count.c index 45bbc59..c405dad 100644 --- a/APP/Service/service_error_count.c +++ b/APP/Service/service_error_count.c @@ -23,6 +23,7 @@ char error_conut_state; #define COUNT_STATE 1 #define ERROR_COUNT_SPEED 1.5 +//#define ERROR_COUNT_SPEED 3 diff --git a/PROJECT/OS2.ewp b/PROJECT/OS2.ewp index ee7b438..571960f 100644 --- a/PROJECT/OS2.ewp +++ b/PROJECT/OS2.ewp @@ -826,7 +826,7 @@