Gear齿轮运行功能

 

 

 

 

 

 

 

 

 

 

 

 

 

 

下面是具体的程序：

1，建立一个枚举：

 

 

//{attribute 'qualified_only'}
//{attribute 'strict'}
TYPE AXIS_Motion :
(
    eAxisMotionNoReady     := 00,
    eAxisMotionReady    := 10,
    eAxisMotionGearIn     := 20,
    eAxisMotionVelocity    := 30,
    eAxisMotionGearOut    := 40,
    eAxisMotionStop        := 50
);
END_TYPE

2，建立一些相关的全局变量：

 

 

VAR_GLOBAL
    //轴控基本功能块
    MC_PowerMaster        :    MC_Power;
    MC_PowerSlave        :    MC_Power;
    
    MC_MoveVelMaster    :    MC_MoveVelocity;
    
    MC_AxisGearIn        :    MC_GearIn;
    MC_AxisGearOut        :    MC_GearOut;
    
    MC_StopMaster        :    MC_Stop;
    MC_StopSlave        :    MC_Stop;    
    
    h_xGearIn            :    BOOL;
    h_xGearOut            :    BOOL;
    h_xStop                :    BOOL;
    h_xMasterRun        :    BOOL;
    
    g_fMasterVel        :    REAL        := 100;
    g_fMasterAcc        :    REAL        := 1000;
    g_fMasterDec        :    REAL        := 1000;
    
    g_xVelExcute        :    BOOL;
    g_xGearInExcute        :    BOOL;
    
    g_xMasterStop        :    BOOL;
    g_xSlaveStop        :    BOOL;
    
    g_xGearOutExcute    :    BOOL;            
    
END_VAR

3，轴控制程序：

 

 

PROGRAM EtherCAT_PRG
VAR
    
END_VAR

//上电默认 伺服使能
MC_PowerMaster(
    Axis:=             Master, 
    Enable:=         , 
    bRegulatorOn:=     , 
    bDriveStart:=     , 
    Status=>         , 
    bRegulatorRealState=> , 
    bDriveStartRealState=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );
    
MC_PowerSlave(
    Axis:=             Slave, 
    Enable:=         , 
    bRegulatorOn:=     , 
    bDriveStart:=     , 
    Status=>         , 
    bRegulatorRealState=> , 
    bDriveStartRealState=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );

MC_AxisGearIn(
    Master:=             Master, 
    Slave:=             Slave, 
    Execute:=             g_xGearInExcute, 
    RatioNumerator:=     900, 
    RatioDenominator:=     1000, 
    Acceleration:=         10000, 
    Deceleration:=         10000, 
    Jerk:= , 
    InGear=> , 
    Busy=> , 
    CommandAborted=> , 
    Error=> , 
    ErrorID=> );

MC_AxisGearOut(
    Slave:=             Slave, 
    Execute:=             g_xGearOutExcute, 
    Done=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );
    
MC_MoveVelMaster(
    Axis:=                 Master, 
    Execute:=             g_xVelExcute, 
    Velocity:=             g_fMasterVel, 
    Acceleration:=         g_fMasterAcc, 
    Deceleration:=         g_fMasterDec, 
    Jerk:= , 
    Direction:=         relative, 
    InVelocity=> , 
    Busy=> , 
    CommandAborted=> , 
    Error=> , 
    ErrorID=> );    
    
MC_StopMaster(
    Axis:=             Master, 
    Execute:=         g_xMasterStop, 
    Deceleration:=     g_fMasterDec, 
    Jerk:=             , 
    Done=>             , 
    Busy=> , 
    Error=> , 
    ErrorID=> );    

MC_StopSlave(
    Axis:=                 Slave, 
    Execute:=             g_xSlaveStop, 
    Deceleration:=         g_fMasterDec, 
    Jerk:= , 
    Done=> , 
    Busy=> , 
    Error=> , 
    ErrorID=> );

4，控制逻辑程序：

 

 

 

 

CASE iState OF
    eAxisMotionNoReady:
        //未准备好,复位相关参数
        g_xVelExcute            := FALSE;
        g_xGearInExcute            := FALSE;
        g_xMasterStop            := FALSE;
        g_xSlaveStop            := FALSE;
        g_xGearOutExcute        := FALSE;        
        
        iState                    := eAxisMotionReady;        
    
    eAxisMotionReady:
        MC_PowerMaster.Enable         := TRUE;
        MC_PowerMaster.bRegulatorOn := TRUE;
        MC_PowerMaster.bDriveStart    := TRUE;

        MC_PowerSlave.Enable         S= TRUE;
        MC_PowerSlave.bRegulatorOn     S= TRUE;
        MC_PowerSlave.bDriveStart    S= TRUE;
        //两轴使能成功
        IF MC_PowerMaster.Status = TRUE AND MC_PowerSlave.Status = TRUE THEN
            iState                    := eAxisMotionGearIn;
        END_IF
        
    eAxisMotionGearIn :
        //轴在standstill状态
        IF Master.nAxisState = 3 AND Slave.nAxisState = 3 THEN
            g_xGearInExcute            := TRUE;
        END_IF    
        IF MC_AxisGearIn.InGear = TRUE THEN
            iState                := eAxisMotionVelocity;
        END_IF    
    eAxisMotionVelocity:
        g_xVelExcute                := h_xMasterRun;
        g_fMasterVel                := 1000;
        g_fMasterAcc                := 1000;
        g_fMasterDec                := 1000;
        IF MC_MoveVelMaster.InVelocity = TRUE THEN
            iState                    := eAxisMotionGearOut;            
        END_IF
        
    eAxisMotionGearOut:
        //等待进行脱耦合操作
        g_xGearOutExcute            := h_xGearOut;
        
        IF MC_AxisGearOut.Done = TRUE THEN
            iState                    := eAxisMotionStop;                
        END_IF
    eAxisMotionStop    :
        g_xMasterStop                := h_xStop;
        g_xSlaveStop                := h_xStop;
        iState                        := eAxisMotionNoReady;        
END_CASE