APP/Device/Device_rotate/rotate_servo.c

@@ -59,56 +59,32 @@ 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) {
-    if ( motorType == horiMotorType )//水平电机
+        if (speed > PTZ_HORI_MOTOR_MAX_SPEED) {
-    {
+            speed = PTZ_HORI_MOTOR_MAX_SPEED;
-        //转换为电机端的r/min
+        }
-        speed = speed * PTZ_HORI_RATIO;
+        else if (speed < PTZ_HORI_MIN_SPEED) {
+            speed = PTZ_HORI_MOTOR_MIN_SPEED;
+        }
         //设置速度，电机的r/min范围-6000~6000
-        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (int16_t)speed)
+        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
-                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);  
-    }
-    /*垂直电机*/
-    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);
     }
-  
+    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
+        //设置速度，电机的r/min范围-6000~6000
-//    static float speedTemp;
+        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
-//    speed = speed * PTZ_HORI_RATIO + 0.5;
+                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
-//    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);
-//    }
     
 }
 
@@ -123,15 +99,13 @@ 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://伺服电机默认速度为正使云台右转
-             speedTemp = speed;
+        case PTZ_HORI_DIR_STOP:
-             break;
+            break;    
         case PTZ_HORI_DIR_LEFT:
-            speedTemp = -speed;
+            speed = -speed;
-            break;
         default:
             break;
     }
@@ -141,11 +115,10 @@ 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);
+    set_speed_to_servoMotor(horiMotorType, speed);
-//    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_speed_set = speed;
     //设定转动方向
     g_ptz.hori_direction_last = g_ptz.hori_direction_set;
     g_ptz.hori_direction_set = direction;
@@ -162,14 +135,17 @@ 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;
 
     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);
     g_ptz.hori_speed_set = 0;
     g_ptz.hori_speed_actual = 0;
   
@@ -177,10 +153,10 @@ 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); //失能电机 
   //电子稳定
 #ifdef PTZ_ELECTRIC_STABLE_L6235D
   ptz_hori_electric_stable_init();
@@ -196,12 +172,10 @@ 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:
-            break;
+            speed = -speed;
         default:
             break;
     }
@@ -230,13 +204,17 @@ 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;
   
@@ -244,9 +222,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

APP/Device/Device_speed/servoMotor_recv.c

@@ -251,11 +251,13 @@ 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)
         {
+            // H_MOTOR_STOP;
             continue;
         }
-        stopTimeOut(H_MOTOR);
+//        OSTimeDlyHMSM(0u, 0u, 0u, 5u);
         //释放信号量，通知能发送一次
         OSSemPost(g_horiSpeedMutex); 
     }
@@ -272,11 +274,13 @@ 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);  
     }

APP/Device/Device_speed/speed_to_servoMotor.c

@@ -310,7 +310,6 @@ static void ptz_hori_servo_task()
         else {
             pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task  no data...\n\r");
         }
-        OSTimeDlyHMSM(0u, 0u, 0u, 10u);
     }
 }
 
@@ -347,7 +346,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);
+        
     }
 }
 
@@ -547,6 +546,7 @@ 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,16 +573,10 @@ void init_speed_module(void)
     //                       , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
     // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
     
-     servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
+     servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
-                             , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+                             , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
-     servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
+     servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
-                             , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+                             , 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);
 }
 

APP/Service/service_error_count.c

@@ -23,7 +23,6 @@ char error_conut_state;
 #define COUNT_STATE   1
 
 #define ERROR_COUNT_SPEED 1.5
-//#define ERROR_COUNT_SPEED 3
 
 
 

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;

PROJECT/OS2.ewp

@@ -826,7 +826,7 @@
                 </option>
                 <option>
                     <name>IlinkIcfFile</name>
-                    <state>D:\CompanyCode\newPro\servoMotor_xr\BSP\IAR\GD32F450xE.icf</state>
+                    <state>D:\psx\Pan-Tilt\1.software\HY\new_ptz\servoMotor\BSP\IAR\GD32F450xE.icf</state>
                 </option>
                 <option>
                     <name>IlinkIcfFileSlave</name>