发送启动命令，仿真可以(时间足够),运行不行（某个地方时间没给够）

2025-10-18 17:20:02 +08:00
5 changed files with 74 additions and 39 deletions
--- a/APP/Device/Device_rotate/rotate_servo.c
+++ b/APP/Device/Device_rotate/rotate_servo.c
@ -61,31 +61,52 @@ 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 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;
-        }
-   
+    /*垂直电机*/
+    else
+    {
        //设置速度，电机的r/min范围-6000~6000
-        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
+        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);
+//    }
+    
 }

 /*
@ -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:
+             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();
--- 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); 
    }
--- 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);
 }

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



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