//----色標中斷信號響應處理：將傳送帶軸當前位置存放到位置緩存
IF SensorTriger=1 THEN
IF (BufDeep<0) THEN
BufDeep:=0; //防止出錯
END_IF
IF BufDeep>=2 THEN
BufDeep:=2; //最多只支持2個緩存，多于2個，丟棄，防止出錯
END_IF
CASE BufDeep OF //--緩存指針，最多記住3個標志
0:
TargetPointArry0:=Axis_CONVERY.fActPosition;
1:
TargetPointArry1:=Axis_CONVERY.fActPosition;
2:
TargetPointArry2:=Axis_CONVERY.fActPosition;
END_CASE
BufDeep:=BufDeep+1;
SensorTriger:=0;
END_IF
//-----自由計時器刷新，可用于判斷超時錯誤-------->>--
IF PowerupTimerS < 32700 THEN
PowerupTimerS:=PowerupTimerS+2; //EtherCAT Period is 2ms
END_IF
//----按系統運行狀態0~5分別處理---------------->>
IF Sysstatus<0 OR Sysstatus>5 THEN //萬一狀態指針錯誤，糾正為重新開始運行
Sysstatus:=0;
END_IF
CASE Sysstatus OF
// System status: 0=Selfchecking; 1=Error; 2=Stopped; 3=Man-Jogging; 4=SlaveAxis Origining;
// 5=MasterAxis firstrun;
0: //上電自診斷，伺服使能 Self checking
IF NOT Inited THEN
//-----使能兩個伺服驅動器----->>------
MC_Power2(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis_FlyCut,);
MC_Power1(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis_CONVERY,);
MC_Power3(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis_ZJ,);
Inited:= MC_Power1.Status AND MC_Power2.Status AND MC_Power3.Status;
//-----超時錯誤判斷------>>-----
IF (PowerupTimerS> 10000) AND (Ethercat.xConfigFinished = FALSE) THEN
SysStatuMark:=1; //EtherCAT Network/Cable Error
Sysstatus:=1; //Error State
END_IF
IF (PowerupTimerS> 12000) AND (Ethercat.xError = TRUE) THEN
SysStatuMark:=2; //EtherCAT Network/Cable Error
Sysstatus:=1; //Error State
END_IF
ELSE
Sysstatus :=2; //If two servo can be ENABLED, Switch to statu 2;
SysStatuMark:=0; //OK
END_IF
1: //停機告警狀態 Stopped,Error Alarming
AxisConvy_RUN:=0;
IF Reset_key=TRUE THEN //----如果手動復位按鈕，
Sysstatus:=0; //--退回到起始狀態
PowerupTimerS:=0; //--重新計時
SysStatuMark:=0; //--清除告警
IF EtherCAT.xConfigFinished=FALSE THEN
EtherCAT.xRestart :=Reset_key; //---如果EtherCAT網絡有問題，重啟網絡
END_IF
Reset_key:=FALSE;
END_IF
IF Setup_key=TRUE AND SysStatuMark=0 THEN //----系統正常的情況下，如果手動調試按鈕，轉到調試狀態
Sysstatus:=3;
END_IF
//Setup_key:=TRUE;
IF Setup_key=FALSE AND SysStatuMark=0 THEN //----系統正常的情況下，如果手動調試按鈕，轉到調試狀態
Sysstatus:=2;
END_IF
2: //正常停機 Stopped, Normal
AxisConvy_RUN:=0;
IF Axis_CONVERY.nAxisState=4 OR Axis_CONVERY.nAxisState=5 THEN
AxisConvery_Stop:=1; //此處給出停機標志
END_IF
IF Axis_FlyCut.nAxisState=4 OR Axis_FlyCut.nAxisState=5 THEN
AxisFlyCut_Stop:=1; //此處給出停機標志
END_IF
IF Setup_key= TRUE THEN
Sysstatus:= 3; //goto JOG and Setting
END_IF
IF Run_key= TRUE THEN
Sysstatus:= 4; //Proparing to run
Run_key:=0;
Run1Cmd:=1;
END_IF
3: // Manual Jogging and setting, Omitted because of no Set_Key here
MC_Jog1( Axis:= Axis_Convery, JogForward:= JOG_AxisConvery_for, JogBackward:=JOG_AxisConvery_bak, Velocity:= 100,Acceleration:= 1000, Deceleration:= 1000, );
MC_Jog2( Axis:= Axis_FlyCut, JogBackward:= JOG_AxisFlyCut_for,JogBackward:=JOG_AxisFlyCut_bak, Velocity:= 100,Acceleration:= 1000, Deceleration:= 1000, );
IF FlyZero_key= TRUE THEN
MC_SetPosition1(Axis:= Axis_FlyCut,Execute:=TRUE,Mode:=FALSE,);
FlyZero_key:= FALSE;
END_IF
IF Setup_key= FALSE AND JOG_AxisConvery_for= FALSE AND JOG_AxisFlyCut_for=FALSE AND JOG_AxisConvery_bak= FALSE AND JOG_AxisFlyCut_bak=FALSE THEN
Sysstatus:= 2;
END_IF
4: //SlaveAxis Adjusting, 若飛剪軸不在等待位置，先回到原點
MC_MoveAbsolute2(Axis:= Axis_FlyCut ,Execute:=Run1Cmd ,Position:=0 ,Velocity:=360 ,Acceleration:=3600 ,Deceleration:=3600 , );
Run1Cmd :=0;
//MC_Home1(Axis:= Axis_FlyCut, Execute:= Run1Cmd, Position:= 0, );
IF MC_MoveAbsolute2.done=TRUE THEN
MC_MoveAbsolute2(Axis:= Axis_FlyCut ,Execute:=FALSE ,);
CAM_TabEnable:=TRUE;//凸輪表使能
Sysstatus:=5; //goto runing
//初始化用于同步起始點的比較寄存器緩沖，這里設計了2級緩沖，Clear CMR buffer------>>--
CMR0:=0;
CMR1:=0;
CMR_Pointer:=0;
//讓自由計時器從新計時，便于檢查色標信號超時判斷--------->>-----
PowerupTimerS:=0;
END_IF
5: //MasterAxis firstRun
//傳送帶開始以速度方式運動，運行速度Vspeed為按時間變化----------------->>--
AxisConvy_RUN:=TRUE;
CAM_InEnable:=TRUE;//凸輪同步使能，由CMR+Offset比對匹配時，觸發同步
END_CASE
//---以下為所有狀態都需要的處理---------------->>
//-----前面作了MC的執行觸發、使能標志的處理，下面統一放在這里------>>
MC_CAMTableSelect1(
Master:= Axis_CONVERY,
Slave:= Axis_FlyCut,
CamTable:= Cam1,
Execute:= CAM_TabEnable,
Periodic:= FALSE,
MasterAbsolute:= FALSE,
SlaveAbsolute:= 1,
Done=> Tbldn,
Busy=> ,
Error=> ,
ErrorID=> ,
CamTableID=> Cam1_ID);
//---比較目標緩沖區的長度，觸發CAM同步的判斷、CamIn的觸發-----
IF CAM_InEnable=TRUE THEN
IF BufDeep >0 THEN
tmp1:=Axis_CONVERY.fActPosition-(TargetPointArry0+SensorCut_Length);
IF tmp1>-0.1 THEN
CamIn_trig:=1;
TargetPointArry0:=TargetPointArry1;
TargetPointArry1:=TargetPointArry2;
BufDeep:=BufDeep-1;
END_IF
//---防錯處理：若傳送帶的當前位置比比較點的位置大，清掉無用的比較點----->>
(* (tmp1>0.1) THEN
TargetPointArry0:=TargetPointArry1;
TargetPointArry1:=TargetPointArry2;
IF BufDeep>0 THEN
BufDeep:=BufDeep-1;
END_IF
END_IF *)
END_IF
END_IF
pp:=Run_key;
MC_CamIn1(
Master:= Axis_CONVERY,
Slave:= Axis_FlyCut,
Execute:= CamIn_trig,
MasterOffset:= 0,
SlaveOffset:= 0,
MasterScaling:= 1,
SlaveScaling:= -1,
StartMode:=0 ,
CamTableID:= Cam1_ID,
VelocityDiff:= 0,
Acceleration:= 30,
Deceleration:= 30, );
CamIn_trig:=FALSE; //
MC_CamOut1(
Slave:= Axis_FlyCut,
Execute:= Stop_key,
Done=> ,
Busy=> ,
Error=> ,
ErrorID=> );
MC_MoveVelocity1(Axis:= Axis_CONVERY , Execute:=AxisConvy_RUN , Velocity:= Vspeed , Acceleration:= 60, Deceleration:=60 , );
//如果2個軸都已停穩3=standstill，返回停機狀態
(*IF Sysstatus=5 AND Axis_CONVERY.nAxisState=3 AND Axis_FlyCut.nAxisState =3 THEN
Sysstatus:=2;
END_IF
*)
//只要有1個軸故障停機1=errorstop，就返回故障停機狀態
IF Axis_CONVERY.nAxisState=1 OR Axis_FlyCut.nAxisState =1 THEN
AxisFlyCut_Stop:=1;
AxisConvery_Stop:=1;
Sysstatus:=1;
END_IF
IF Stop_key=1 THEN
AxisFlyCut_Stop:=1;
AxisConvery_Stop:=1;
AxisConvy_RUN:=0;
Stop_key:=0;
END_IF
//----測試位移----->>
MC_MoveRelative1(
Axis:= Axis_CONVERY,
Execute:= test1,
Distance:=120 ,
Velocity:= Vspeed,
Acceleration:=60 ,
Deceleration:= 60,
);
//----測試位移------>>
MC_MoveRelative2(
Axis:= Axis_FlyCut,
Execute:= test2,
Distance:=360 ,
Velocity:= Vspeed,
Acceleration:=60 ,
Deceleration:= 60,
);
MC_Stop1(Axis:= Axis_CONVERY,Execute:=AxisConvery_Stop,);
MC_Stop2(Axis:= Axis_FlyCut,Execute:=AxisFlyCut_Stop,);
AxisFlyCut_Stop:=0;
AxisConvery_Stop:=0;
MC_Reset1(Axis:= Axis_CONVERY, Execute:= r1, );
MC_Reset2(Axis:= Axis_FlyCut, Execute:= r2, );
MC_Reset3(Axis:= Axis_ZJ, Execute:= r3, );
IF Sysstatus >0 AND ( 飛剪伺服.wstate <>8) THEN
b:=b+1;
SysStatuMark:=2; //第二個伺服網絡斷線
END_IF
IF Sysstatus >0 AND ( 追剪伺服.wstate <>8) THEN
a:=a+1;
SysStatuMark:=3; //第一個伺服網絡斷線
END_IF
//---下面的語句是為了讓MC指令能得到下一次的上升沿------------>>---
MC_MoveAbsolute2(Axis:= Axis_FlyCut ,Execute:=FALSE , );
MC_MoveVelocity1(Axis:= Axis_CONVERY , Execute:=FALSE,);
MC_SetPosition1(Axis:= Axis_FlyCut,Execute:=FALSE,Mode:=FALSE,);
評論