附件下载,需登录可以查看贴内更多信息
您需要 登录 才可以下载或查看,没有账号?立即注册
x
bAuto:=(Mode=1);
bManual:=( Mode=3 OR Mode=4 OR Mode=5);
OperationAbs();
OperationHome();
OPerationJog();
OperationPower();
OperationRel();
OperationReset();
OperationSetPos();
OperationStop();
PostionSaveEroor();
ReadEtherCat();
ReadStatus();
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 绝对定位 ********************************************)
IF Operation.bAlarmReset OR Reset THEN
Status.SoftLimitError:=FALSE;
END_IF;
FOR k:=0 TO MAX_POS BY 1 DO
//判断参数是否在范围;
IF Parameter.AxisAbsPos[k] < Parameter.rSoftLimitMin AND Status.bHomed
THEN
fbPop_Up.Clk := TRUE ;
Status.SoftLimitError:=TRUE;
Show.MesgBox:=WCONCAT(STR1:=AxisName, STR2:="轴Abs定位坐标设置超出软下限位报警");
ELSIF
Parameter.AxisAbsPos[k] > Parameter.rSoftLimitMax AND Status.bHomed
THEN
fbPop_Up.Clk := TRUE ;
Status.SoftLimitError:=TRUE;
Show.MesgBox:=WCONCAT(STR1:=AxisName, STR2:="轴Abs定位坐标设置超出软上限位报警");
END_IF;
//自动定位触发:
IF TRUE
AND (bAuto OR Mode=2)
AND NOT Status.bError //报错汇总
AND NOT E_Stop //急停
AND Status.bPowerOn //轴使能OK
AND Status.bHomed //回原完成
AND NOT Status.bAxisBusy //轴忙
AND NOT Status.SoftLimitError //软极限报错
AND Operation.bABSMovcondition[K] //轴移动允许
AND Operation.bABSAutoTrig[K] //轴自动触发
THEN
MC_MoveAbsolute_Instance.Velocity :=REAL_TO_LREAL( Parameter.AxisAbsVel[K]); //写入定位的速度;
MC_MoveAbsolute_Instance.Position := REAL_TO_LREAL(Parameter.AxisAbsPos[k]); //写入定位的坐标;
bMoveAbs := TRUE; //触发功能块执行;
bAbsCycle := TRUE;
ELSIF
//手动定位触发:
TRUE
AND bManual
AND NOT Status.bError //报错汇总
AND NOT E_Stop //急停
AND Status.bPowerOn //轴使能OK
AND Status.bHomed //回原完成
AND NOT Status.bAxisBusy //轴忙
AND NOT Status.SoftLimitError //软极限报错
AND Operation.bABSMovcondition[K] //轴移动允许
AND Operation.bABSManualTrig[K] //轴手动触发
THEN
MC_MoveAbsolute_Instance.Velocity := REAL_TO_LREAL(Parameter.rManualVel); //写入定位的速度;
MC_MoveAbsolute_Instance.Position :=REAL_TO_LREAL( Parameter.AxisAbsPos[k]); //写入定位的坐标;
bMoveAbs := TRUE; //触发功能块执行;
bAbsCycle := TRUE;
ELSIF
//手动定位触发:
TRUE
AND bManual
AND NOT Status.bError //报错汇总
AND NOT E_Stop //急停
AND Status.bPowerOn //轴使能OK
AND Status.bHomed //回原完成
AND NOT Status.bAxisBusy //轴忙
AND NOT Status.SoftLimitError //软极限报错
AND NOT Operation.bABSMovcondition[K] //轴移动允许
AND Operation.bABSManualTrig[K] //轴手动触发
THEN
fbPop_Up.Clk := TRUE ;
Show.MesgBox:=WCONCAT(STR1:=AxisName, STR2:="轴移动条件不满足");
ELSE
IF bAbsCycle = FALSE THEN
bMoveAbs:= FALSE;
END_IF;
END_IF;
IF Operation.bAlarmReset OR Reset THEN
Operation.bABSManualTrig[K]:=FALSE;
Operation.bABSAutoTrig[K]:=FALSE;
Operation.bHome_HMI:=FALSE;
Operation.bHome_INIT:=FALSE;
Operation.bRELManualTrig:=FALSE;
Operation.bRELAutoTrig:=FALSE;
Operation.bSetPosAutoTrig:=FALSE;
Operation.bSetPosManualTrig:=FALSE;
END_IF;
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(******************************************** Abs功能块用 ********************************************)
MC_MoveAbsolute_Instance(Axis:=Axis_REF^,
Execute:=bMoveAbs,
Acceleration:=REAL_TO_LREAL(Parameter.rAcceleration),
Deceleration:=REAL_TO_LREAL(Parameter.rDeceleration)
);
(******************************************** Abs定位完成 ********************************************)
IF TRUE THEN
IF TRUE
AND Status.bHomed
AND NOT Status.bAxisBusy
AND ABS ( Status.rActPos - Parameter.AxisAbsPos[K]) <arameter.rInPositionRange //判断波动范围
THEN
Status.PostionDone[K] := TRUE ;
ELSE
Status.PostionDone[K] := FALSE ;
END_IF;
END_IF;
(******************************************** Abs定位示教 ********************************************)
IF TRUE
AND Status.bHomed
AND NOT Status.bAxisBusy
AND NOT Status.SoftLimitError
AND Operation.ABSTeach[K]
THEN
Parameter.AxisAbsPos[K]:=Status.rActPos ;
END_IF;
END_FOR;
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 轴回原 ********************************************)
HomeExecute:=(Operation.bHome_HMI OR Operation.bHome_INIT) AND Operation.bHomeMovcondition AND NOT Status.bHoming AND NOT Status.bAxisBusy AND Status.bPowerOn;
//
MC_Home_Instance(Axis:= Axis_REF^ ,
Execute:=HomeExecute
);
MC_Hom_P(CLK:=MC_Home_Instance.Done);
IF MC_Hom_P.Q AND ( Axis_REF^.In.wStatusWord.15 OR Simulate = 1) THEN
Status.bHomed:=TRUE;
END_IF;
IF MC_Home_Instance.Error OR MC_Home_Instance.CommandAborted OR (NOT Axis_REF^.In.wStatusWord.15 AND Simulate = 0 ) THEN
Status.bHomed:=FALSE;
END_IF;
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 轴点动 ********************************************)
IF bManual THEN
MC_MoveJog_Instance.Velocity:=REAL_TO_LREAL(Parameter.rManualVel);
END_IF;
IF bAuto THEN
MC_MoveJog_Instance.Velocity:=REAL_TO_LREAL(Parameter.rManualVel*10.0);
END_IF;
MC_MoveJog_Instance(Axis:=Axis_REF^ ,
JogForward:=Operation.bJOGMovcondition[0] AND NOT Status.bAxisBusy AND Status.bPowerOn AND( ((Operation.bJOG_ManualTrig[0] AND NOT Operation.bJOG_ManualTrig[1] )AND bManual) OR
((Operation.bJOG_AutoTrig[0] AND NOT Operation.bJOG_AutoTrig[1] )AND bAuto)),
JogBackward:=Operation.bJOGMovcondition[1]AND NOT Status.bAxisBusy AND Status.bPowerOn AND( ((Operation.bJOG_ManualTrig[1] AND NOT Operation.bJOG_ManualTrig[0] )AND bManual) OR
((Operation.bJOG_AutoTrig[1] AND NOT Operation.bJOG_AutoTrig[0] )AND bAuto)),
//Velocity:=,
Acceleration:=REAL_TO_LREAL(Parameter.rAcceleration),
Deceleration:=REAL_TO_LREAL(Parameter.rDeceleration)
);
(****************************************** 轴使能 ********************************************)
IF TRUE AND NOT Operation.bAxisEnableHMI
AND NOT Axis_REF^.In.wStatusWord.3
AND (
Axis_REF^.In.wStatusWord.4
AND Axis_REF^.wCommunicationState=100
AND EtherCATConnectOK
OR Simulate <> 0
)
THEN
bEnableTonReady := TRUE ;
ELSE
bEnableTonReady := FALSE ;
END_IF;
//延时使能;
EnableTon(In:=bEnableTonReady, PT:=TIME#100MS);
//调用功能块
MC_Power_Instance( Axis:=Axis_REF^,
Enable:= 1,
bRegulatorOn:=EnableTon.Q,
bDriveStart:= EnableTon.Q,
Status=>Status.bPowerOn
);
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 相对定位 ********************************************)
IF TRUE
AND bAuto
AND NOT Status.bError //报错汇总
AND NOT E_Stop //急停
AND Status.bPowerOn //轴使能OK
AND Status.bHomed //回原完成
AND NOT Status.bAxisBusy //轴忙
AND Operation.bMovRelMovcondition //轴移动允许
AND Operation.bRELAutoTrig //轴自动触发
THEN
MC_MoveRelative_Instance.Velocity:=REAL_TO_LREAL(Parameter.rMoveRelVelocity); //写入定位的速度;
MC_MoveRelative_Instance.Distance :=REAL_TO_LREAL(Parameter.rMoveRelDistance); //写入定位的坐标;
bMoveRel := TRUE ; //触发功能块执行;
bRelCycle := TRUE ;
ELSIF
//手动定位触发:
TRUE
AND bManual
AND NOT Status.bError //报错汇总
AND NOT E_Stop //急停
AND Status.bPowerOn //轴使能OK
AND Status.bHomed //回原完成
AND NOT Status.bAxisBusy //轴忙
AND Operation.bMovRelMovcondition //轴移动允许
AND Operation.bRELManualTrig //轴手动触发
THEN
MC_MoveRelative_Instance.Velocity:=REAL_TO_LREAL(Parameter.rMoveRelVelocity); //写入定位的速度;
MC_MoveRelative_Instance.Distance :=REAL_TO_LREAL(Parameter.rMoveRelDistance); //写入定位的坐标;
bMoveRel := TRUE ; //触发功能块执行;
bRelCycle := TRUE ;
ELSE
IF bRelCycle = FALSE THEN
bMoveRel:= FALSE;
END_IF;
END_IF;
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(******************************************** Rel功能块用 ********************************************)
MC_MoveRelative_Instance(Axis:=Axis_REF^,
Execute:=bMoveRel,
Acceleration:=REAL_TO_LREAL(Parameter.rAcceleration),
Deceleration:=REAL_TO_LREAL(Parameter.rDeceleration) );
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 轴复位 ********************************************)
MC_Reset_Instance(Axis:=Axis_REF^,
Execute:=(Operation.bAlarmReset OR Reset) AND Axis_REF^.wCommunicationState=100 );
ReinitDrive(
Axis:= Axis_REF^,
bExecute:= (Operation.bAlarmReset OR Reset) AND Axis_REF^.wCommunicationState<>100
);
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(******************************************** 坐标设定模式 ********************************************)
MC_SetPosition_Instance(
Axis:=Axis_REF^,
Execute:=(Operation.bSetPosManualTrig OR Operation.bSetPosAutoTrig ) AND Status.bHomed AND NOT Status.bAxisBusy ,
Position:=REAL_TO_LREAL(Parameter.rSetPos),
Done=>Status.bSetPosDone
);
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 轴停止 ********************************************)
MC_Stop_Instance( Axis:= Axis_REF^ ,
Execute:=E_Stop,
Deceleration:=REAL_TO_LREAL(Parameter.rDeceleration),
//Jerk:=,
Done=>Status.bStopDone);
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(******************************************** 判断坐标位置 ********************************************)
IF
bConflictEnable THEN
// 手自动冲突状态记录
fbR_TRIG2(CLK:= bAuto );
IF fbR_TRIG2.Q THEN
bAxisStateMark := TRUE ;
END_IF;
//记录延时复位;
BusyTimeOut(IN:= bAxisStateMark AND Axis_REF^.nAxisState=Standstill AND Status.bConflictErr = FALSE , PT:= T#100MS);
//初始化完成;
fbR_TRIG3(Clk:=InitDone );
//滤波时间;
fbTon2(In:=Status.bAxisBusy = FALSE, PT:= T#200MS);
//记录停下来的位置;
IF BusyTimeOut.Q OR fbR_TRIG3.Q
THEN
bAxisStateMark := FALSE ;
Status.rConflictStopPos := Status.rActPos ;
END_IF;
// 手自动冲突报警
// IF bManual AND InitDone THEN
// Status.bConflictErr := ABS( Status.rActPos - Status.rConflictStopPos) > 1.0 AND fbTon2.Q;
// END_IF;
// 手自动冲突报警
FOR Index:=0 TO MAX_POS BY 1 DO
IF bMoveAbs AND Operation.bABSAutoTrig[Index] THEN
PositionIndex:=Index;
EXIT;
END_IF
END_FOR
Status.PosIndex:=PositionIndex;
Status.bConflictErr:=StartSingel AND bAuto AND InitDone AND NOT Status.PostionDone[PositionIndex] AND Axis_REF^.nAxisState=Standstill ;
//报警弹窗
fbR_TRIG6(Clk:=Status.bConflictErr);
IF fbR_TRIG6.Q THEN
Show.MesgBox :=WCONCAT(STR1:=AxisName, STR2:="轴干涉提示" );
fbPop_Up.Clk := TRUE ;
END_IF;
//把当前位置传给手自动冲突位置
IF
bClearConflict THEN
Status.rConflictStopPos := Status.rActPos ;
END_IF;
ELSE
bAxisStateMark := FALSE ;
Status.bConflictErr := FALSE ;
Status.rConflictStopPos := 0;
END_IF;
//弹窗处理
fbPop_Up();
//触发弹窗;
IF fbPop_Up.Q THEN
Show.MesgboxTrig :=TRUE;
END_IF;
//消除提示窗信息
IF Show.MesgboxTrig = FALSE THEN
fbPop_Up.CLK := FALSE ;
Clear(Show.MesgBox );
END_IF;
// IF P_First_Run THEN
// Show.MesgboxTrig :=FALSE;
// fbPop_Up.CLK :=FALSE;
// END_IF;
//从EtherCAT 网络上拥有指定从站的CoE (*) 对象中读取值。
SdoObjIndex := 24639 ;//16#603F Error Code
SdoObjSubindex := 0 ;//16#0000
EC_CoESDORead1(
xExecute:=Axis_REF^.In.wStatusWord.3 ,
//xAbort:= ,
//usiCom:= ,
uiDevice:=NodeAdr ,
//usiChannel:= ,
wIndex:=SdoObjIndex ,
bySubindex:=SdoObjSubindex ,
udiTimeOut:=500 ,
//xDone=> ,
//xBusy=> ,
//xError=> ,
//eError=> ,
//udiSdoAbort=> ,
dwData=>EC_CoESDORead_Data ,
//usiDataLength=>
);
IF Axis_REF^.In.wStatusWord.3 THEN
Status.nErrorID:=DWORD_TO_UDINT( EC_CoESDORead_Data);
IF Simulate = 0 THEN
Status.bError:=TRUE;
END_IF;
ELSE
Status.nErrorID:=UDINT#0;
END_IF;
//获取轴错误编号
IF Axis_REF^.nAxisState=errorstop THEN
//Status.nNCErrID := DWORD_TO_UDINT(Axis_REF.MFaultLvl.Code);
IF Simulate = 0 THEN
Status.bError:=TRUE;
END_IF;
ELSE
Status.nNCErrID:=0;
END_IF;
//轴名称读取
Status.AxisName:=AxisName;
//轴状态Busy'
Status.bAxisBusy:= MC_Home_Instance.Busy OR
MC_MoveAbsolute_Instance.Busy OR
MC_MoveJog_Instance.Busy OR
MC_MoveRelative_Instance.Busy ;
//轴状态功能块错误
Status.bFbError:=MC_Home_Instance.Error OR
MC_MoveAbsolute_Instance.Error OR
MC_MoveJog_Instance.Error OR
MC_MoveRelative_Instance.Error OR
MC_Stop_Instance.Error OR
MC_Reset_Instance.Error OR
MC_Power_Instance.Error;
IF Status.bError AND Operation.bAlarmReset THEN
Status.bError:=FALSE;
END_IF;
//定位边缘信号处理
bAbsCycle:= FALSE ;
bRelCycle:=FALSE;
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
(****************************************** 轴信号综合刷新 ********************************************)
//轴正限位
Status.bPotlimitSensor := Axis_REF^.In.dwDigitalInputs.1;
//轴负限位
Status.bNotLimitSensor := Axis_REF^.In.dwDigitalInputs.0;
//轴原点
Status.bHomeSensor := Axis_REF^.In.dwDigitalInputs.2;
//轴回原中
Status.bHoming := Axis_REF^.nAxisState=homing ;
//轴当前坐标
Status.rActPos := LREAL_TO_REAL(Axis_REF^.fActPosition);
//轴当前转矩
Status.rActTorque := LREAL_TO_REAL(Axis_REF^.fActTorque);
//轴当前速度
Status.rActVelo := LREAL_TO_REAL(Axis_REF^.fActVelocity);
//轴正方向指令中
// X_TEMP:= ;
Status.bDirPosi := NOT Status.bDirNega;
//轴负方向指令中
Status.bDirNega := Axis_REF^.nDirection=negative;
//轴准备好
Status.bReady :=Axis_REF^.wCommunicationState=100;
//轴静止
Status.Standstill := Axis_REF^.nAxisState=Standstill;
//轴当前状态
Status.nAxisState:=INT_TO_UINT(Axis_REF^.nAxisState);
//(---------------------------------------------------------------------------------------------------------------------------------------------------------)
免责声明:内容为网友自行发布或者来自互联网资源或者参考文献,如果侵犯了您的权益,请联系站长 1304546267@qq.com,我们会及时删除侵权内容,感谢您的理解! |