(*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; //位置 *)