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 @@