Ⅲ型系统外部轴---非同步轴

 

 

(*PDO非同步轴外部轴*)
//外部轴运动模块,暂时支持高创、华数驱动
PROGRAM PDOMoveExtAxis
VAR
//操作相关变量
    bEnable:                     BOOL;
    bdisable:                    BOOL;    
    bReset:                      BOOL;

    iCounter:                    INT;                            //计数器
    iExtNum:                     INT;                            //外部轴个数
    
    diPos:                       DINT:=0;                        //位置增量,决定速度
    
//控制字
    uiControlWordOutput:         UINT;                           //控制字
    
//PDO      
    abControlWordBit:            ARRAY[0..15] OF BOOL;           //控制字
    abStatusWordBit:             ARRAY[0..15] OF BOOL;           //状态字
    
//实例化
    aPowerPDO:                   ARRAY[0..8] OF PDO.PowerPDO;    //PDO使能模块
    
//获取TCP速度
    lrVel:                       LREAL;
    alrVel:                      ARRAY[0..8] OF LREAL;
    Data:                        PDO.TMP.Data;
    
    //Home
    diExtAxisHome:               DINT;
    diSetPosMid:                 DINT;
    diPosMid:                    DINT;
    lrVelExt:                    LREAL;
    bRuning:                     BOOL;   //运行标志位
    EnableDone:                  BOOL;   //上使能完成标志
    abEnableLed:                 BOOL;
    bAxisNum:                    INT;    //机器人内部轴个数，视图填写
    statusNum:                   INT;    //状态机用
    insideAxisType0:              INT;
    insideAxisType1:              BOOL;    //内部轴类型
    insideAxisType2:              BOOL;    //内部轴类型
    insideAxisType3:              BOOL;    //内部轴类型
    outsideAxisType0:             INT;
    outsideAxisType1:             BOOL;    //外部轴类型
    outsideAxisType2:             BOOL;    //外部轴类型
    bstart:                       BOOL;
END_VAR


//1、通过CA来拿去非同步或者同步轴的开关状态,已经开放到全局变量
//内部轴
IF insideAxisType1=TRUE AND insideAxisType2=FALSE AND insideAxisType3=FALSE THEN
    insideAxisType0:=1;//高创
ELSIF insideAxisType1=FALSE AND insideAxisType2=TRUE AND insideAxisType3=FALSE THEN
    insideAxisType0:=2;//清能德创
ELSIF insideAxisType1=FALSE AND insideAxisType2=FALSE AND insideAxisType3=TRUE THEN
    insideAxisType0:=3;//华数
END_IF

IF outsideAxisType1=TRUE AND outsideAxisType2=FALSE THEN
    outsideAxisType0:=1;//高创
ELSIF outsideAxisType1=FALSE AND outsideAxisType2=TRUE THEN
    outsideAxisType0:=2;//华数
END_IF

GC_iInternalAxisNum:= bAxisNum;
//GC_bSyncAxis:= TRUE;    //视图里启动
//目前只支持外部轴是高创类型的
IF GC_bSyncAxis THEN //启动信号
    //判断是否具有外部轴
    IF UINT_TO_INT(Motion.auiActualNOSlavesServo[0])> GC_iInternalAxisNum     THEN
         iExtNum:= UINT_TO_INT(Motion.auiActualNOSlavesServo[0]) - GC_iInternalAxisNum;
    ELSE
         RETURN;
    END_IF
    IF insideAxisType0=1 AND outsideAxisType0=1 THEN
        iCounter:= 6;
    ELSIF insideAxisType0=2 AND outsideAxisType0=1 THEN
        iCounter:= 0;
    ELSIF insideAxisType0=3 AND outsideAxisType0=1 THEN
        iCounter:= 0;
    END_IF
    
    ACT_CDHD();

   (*
    FOR iCounter:= 0 TO (iExtNum-1) BY 1 DO
          CASE  UINT_TO_INT(Motion.auiServoType[GC_iInternalAxisNum + iCounter,0]) OF 
             Servotype.ST_CDHD:       //CDHD
                                      ACT_CDHD();
        
             Servotype.ST_HSS_LDE:    //HSS_LDE
                                      ACT_HSS_LDE();
          END_CASE
    END_FOR
    *)
END_IF

//非同步轴控制
//获取TCP合成速度
//Data.GetVelBaseCoord(VelBase:= ADR(lrVel));

abStatusWordBit:= UINT_16_TO_BOOL(GC_aPdoCDHD[iCounter].StatusWord);

//未使能
CASE statusNum OF
    0:
       IF NOT(abStatusWordBit[2]) THEN
           diExtAxisHome:= GC_aPdoCDHD[iCounter].PositionActualValue;
       END_IF
       statusNum:=100;
    100:
       ;
END_CASE

//使能控制
aPowerPDO[iCounter](
    bEnable:= bEnable, 
    bdisable:= bdisable, 
    abStatusWordBit:= abStatusWordBit, 
    abControlWordBit=> abControlWordBit, 
    bServoStatus=> abStatusWordBit[iCounter]);
IF uiControlWordOutput=15 THEN
    EnableDone:=TRUE;
END_IF     
//复位
IF bReset AND abStatusWordBit[3] THEN
    bReset:= FALSE;
   IF abControlWordBit[7] THEN
       abControlWordBit[7] := FALSE;
   ELSE
       abControlWordBit[7] := TRUE;
   END_IF
ELSE
    abControlWordBit[7] := FALSE;
END_IF

//bool-->Uint
GC_aPdoCDHD[iCounter].ControlWord:= BOOL_16_TO_UINT(abControlWordBit);

//下发
GC_aPdoCDHD[iCounter].ModesOfOperation:= 8;      //位置控制模式

IF bstart=TRUE THEN
    //diSetPosMid:=diSetPosMid+  LREAL_TO_DINT(R100 * lrVel*0.004 * (1000000/360));
    diSetPosMid:=diSetPosMid+  LREAL_TO_DINT(lrVel*0.004 * (1000000/360));
END_IF
GC_aPdoCDHD[iCounter].TargetPosition:=  diExtAxisHome + diSetPosMid ;//* diPos;   //位置

//外部轴得速度
lrVelExt:= (DINT_TO_LREAL(GC_aPdoCDHD[iCounter].PositionActualValue - diPosMid)/(1000000/360.0)/0.004);
IF lrVelExt>1 OR lrVelExt<-1 THEN   //判断附加轴是否运动
    bRuning:=TRUE;                  //运行标志
END_IF
//上一个周期的实际位置
diPosMid:=  GC_aPdoCDHD[iCounter].PositionActualValue;

//非同步轴控制
//获取TCP合成速度
//Data.GetVelBaseCoord(VelBase:= ADR(lrVel));

abStatusWordBit:= UINT_16_TO_BOOL(GC_aPdoHSS_LDE[iCounter].StatusWord);

//未使能
CASE statusNum OF
    0:
       IF NOT(abStatusWordBit[2]) THEN
           diExtAxisHome:= GC_aPdoHSS_LDE[iCounter].PositionActualValue;
       END_IF
       statusNum:=100;
    100:
       ;
END_CASE

//使能控制
aPowerPDO[iCounter](
    bEnable:= bEnable, 
    bdisable:= bdisable, 
    abStatusWordBit:= abStatusWordBit, 
    abControlWordBit=> abControlWordBit, 
    bServoStatus=> abStatusWordBit[iCounter]);
IF uiControlWordOutput=15 THEN
    EnableDone:=TRUE;
END_IF     
//复位
IF bReset AND abStatusWordBit[3] THEN
    bReset:= FALSE;
   IF abControlWordBit[7] THEN
       abControlWordBit[7] := FALSE;
   ELSE
       abControlWordBit[7] := TRUE;
   END_IF
ELSE
    abControlWordBit[7] := FALSE;
END_IF

//bool-->Uint
GC_aPdoHSS_LDE[iCounter].ControlWord:= BOOL_16_TO_UINT(abControlWordBit);

//下发
GC_aPdoHSS_LDE[iCounter].ModesOfOperation:= 8;      //位置控制模式

IF bstart=TRUE THEN
    //diSetPosMid:=diSetPosMid+  LREAL_TO_DINT(R100 * lrVel*0.004 * (1000000/360));
    diSetPosMid:=diSetPosMid+  LREAL_TO_DINT(lrVel*0.004 * (1000000/360));
END_IF
GC_aPdoHSS_LDE[iCounter].TargetPosition:=  diExtAxisHome + diSetPosMid ;//* diPos;   //位置

//外部轴得速度
lrVelExt:= (DINT_TO_LREAL(GC_aPdoCDHD[iCounter].PositionActualValue - diPosMid)/(1000000/360.0)/0.004);
IF lrVelExt>1 OR lrVelExt<-1 THEN   //判断附加轴是否运动
    bRuning:=TRUE;                  //运行标志
END_IF
//上一个周期的实际位置
diPosMid:=  GC_aPdoHSS_LDE[iCounter].PositionActualValue;



(*
//PDO、附加轴处理
//反馈
//alrServoOutPara[iCounterCDHD,0]:= GC_aPdoHSS_LDE[iCounterCDHD].PositionActualValue;         //位置信息

//alrServoOutPara[iCounterCDHD,7]:= 8;                                                      //控制模式

abStatusWordBit:= UINT_16_TO_BOOL(GC_aPdoHSS_LDE[iCounter].StatusWord);



IF abStatusWordBit[1] THEN
    ;//alrServoOutPara[iCounterCDHD,6]:= 3;                                                 //轴状态  
ELSE
    ;//alrServoOutPara[iCounterCDHD,6]:= 0;                                                 //轴状态  
END_IF

//驱动故障
IF  abStatusWordBit[3] THEN
    //报错状态
    //alrServoOutPara[iCounterCDHD,8]:= GC_aPdoHSS_LDE[iCounterCDHD].ErrorCode+256*256*256*128*1;
    //alrServoOutPara[iCounterCDHD,6]:= 1;                                                 //轴状态  
ELSE
    //alrServoOutPara[iCounterCDHD,8]:= 0;
END_IF

//使能
aPowerPDO[iCounter](
            bEnable:= bEnable, 
            bdisable:= bdisable, 
            abStatusWordBit:= abStatusWordBit, 
            abControlWordBit=> abControlWordBit, 
            bServoStatus=> abStatusWordBit[iCounter]);

//使能控制
abControlWordBit:= UINT_16_TO_BOOL(uiControlWordOutput);

//复位
IF bReset THEN
   IF abControlWordBit[7] THEN
       abControlWordBit[7] := FALSE;
   ELSE
       abControlWordBit[7] := TRUE;
   END_IF
ELSE
    abControlWordBit[7] := FALSE;
END_IF

//bool-->Uint
GC_aPdoHSS_LDE[iCounter].ControlWord:= BOOL_16_TO_UINT(abControlWordBit);

//下发
GC_aPdoHSS_LDE[iCounter].ModesOfOperation:= 8;      //位置控制模式

GC_aPdoHSS_LDE[iCounter].TargetPosition:=  GC_aPdoHSS_LDE[iCounter].PositionActualValue + diPos;   //位置
*)