ACOPOS 6D使用手册 > mapp 6D > 功能块 > McAcp6D > Function blocks > MC_BR_MoveCyclicPosition

CASE Ctrl.aSh[Ctrl.i].uStep OF

20: (*-- 时序状态机判断 --*)

IF gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].uTime = 0 THEN

(*-- 走点到点 MoveInPlane --*)

Ctrl.aSh[Ctrl.i].uLastCommand := 1; //意味着在MoveInPlane

//Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Shuttle := ADR(Core.Internal.List.Shuttle[Ctrl.i].Ref);//托盘

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Shuttle := Ctrl.aSh[Ctrl.i].pShuttleRef;

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Position.X := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Position.Y := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Velocity := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rSpeed / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Acceleration := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rAcceleration / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.EndVelocity := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rArrivalSpeed / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Mode;

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Path;

Ctrl.aSh[Ctrl.i].rLastMoveInPlanePointX := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX;

Ctrl.aSh[Ctrl.i].rLastMoveInPlanePointY := gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY;

(*-- 边界判断是否执行 --*)(*-- 托盘的运动范围 X:240 ~ 1680 Y：240 ~ 1200 ，原点为240,240--*)

(*-- X[390,1530] Y[255,1185] 带载具最大值最小值范围--*)

IF gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX < MOVING_X_MIN

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX > MOVING_X_MAX

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY < MOVING_Y_MIN

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY > MOVING_Y_MAX THEN

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Execute := FALSE;

Ctrl.aSh[Ctrl.i].uStep := 0;

uStepPath := 0;

g6DControlStatus.sRDICore_Status := '120 → 0 | 20 → 0 MoveInPlane TCPData[Ctrl.i].para_req_path[0].rX rY beyond limit';

ELSE

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Execute := TRUE;

END_IF

//Ctrl.aSh[Ctrl.i].FB_MoveInPlane(); (*-- 功能块执行 --*)

IF Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Acknowledge THEN

(*----------------------- 开始记录标志位 -----------------------*)

Ctrl.aSh[Ctrl.i].bStartRecordDOFlag := TRUE; //清空动作在99步/111步

Ctrl.aSh[Ctrl.i].uPathIndex := Ctrl.aSh[Ctrl.i].uPathIndex + 1;

g6DControlStatus.sRDICore_Status := '120 | 20 → 10 FB_MoveInPlane Start ,Waiting for FB_MoveInPlane.DONE';

Ctrl.aSh[Ctrl.i].uStep := 30;

ELSIF Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Error THEN

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Execute := FALSE;

g6DControlStatus.sRDICore_Status := '120 | 20 → 255 FB_MoveInPlane ERROR';

Ctrl.aSh[Ctrl.i].uStep := 255;

END_IF;

ELSE

(*-- 走轨迹 MoveCyclicPosition --*)

//Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Shuttle := ADR(Core.Internal.List.Shuttle[Ctrl.i].Ref);//托盘;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Shuttle := Ctrl.aSh[Ctrl.i].pShuttleRef;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Parameters.DisableAxis.RX := TRUE;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Parameters.DisableAxis.RY := TRUE;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Parameters.DisableAxis.RZ := TRUE;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Parameters.DisableAxis.Z := TRUE;

(*-- MoveCyclicPosition 初始输入参数需要是0，再Excute --*)

IF Ctrl.aSh[Ctrl.i].uLastCommand <> 2 THEN

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.CyclicPosition.X := 0;

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.CyclicPosition.Y := 0;

ELSE

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.CyclicPosition.X := (gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX - Ctrl.aSh[Ctrl.i].rLastMoveInPlanePointX) / 1000.0; //单位转换 mm转换为m

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.CyclicPosition.Y := (gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY - Ctrl.aSh[Ctrl.i].rLastMoveInPlanePointY) / 1000.0; //单位转换 mm转换为m

END_IF;

(*-- 边界判断是否执行 --*)(*-- 托盘的运动范围 X:240 ~ 1680 Y：240 ~ 1200 ，原点为240,240--*)

(*-- X[390,1530] Y[255,1185] 带载具最大值最小值范围--*)

IF gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX < MOVING_X_MIN

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rX > MOVING_X_MAX

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY < MOVING_Y_MIN

OR gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].rY > MOVING_Y_MAX THEN

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Enable := FALSE;

Ctrl.aSh[Ctrl.i].uStep := 0;

uStepPath := 0;

g6DControlStatus.sRDICore_Status := '120 → 0 | 20 → 0 MoveCyclic TCPData[Ctrl.i].para_req_path[0].rX rY beyond limit';

ELSE

IF Ctrl.aSh[Ctrl.i].uPathIndex <= MIN(Ctrl.aSh[Ctrl.i].uPathTotalNum,REQ_PATH_DATA_NUM) THEN

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Enable := TRUE;

ELSE

Ctrl.aSh[Ctrl.i].bMovingDone := TRUE;

END_IF;

END_IF

IF Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Valid THEN

(*----------------------- 开始记录标志位 -----------------------*)

Ctrl.aSh[Ctrl.i].bStartRecordDOFlag := TRUE; //清空动作在99步/111步

IF Ctrl.aSh[Ctrl.i].uLastCommand <> 2 THEN

Ctrl.aSh[Ctrl.i].uLastCommand := 2;

ELSE

Ctrl.aSh[Ctrl.i].uPathIndex := Ctrl.aSh[Ctrl.i].uPathIndex + 1; (*-- 下一个轨迹点 --*)

END_IF;

IF gTCPData[Ctrl.i].para_req_path[Ctrl.aSh[Ctrl.i].uPathIndex].uTime = 0 THEN

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Enable := FALSE;

END_IF;

ELSIF Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Error THEN

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition.Enable := FALSE;

g6DControlStatus.sRDICore_Status := '120 | 20 → 255 FB_MoveCyclicPosition ERROR';

Ctrl.aSh[Ctrl.i].uStep := 255;

END_IF;

(*----------------------- 判断数据是否完成 -----------------------*)

IF Ctrl.aSh[Ctrl.i].uPathIndex >= MIN(Ctrl.aSh[Ctrl.i].uPathTotalNum,REQ_PATH_DATA_NUM) THEN

Ctrl.aSh[Ctrl.i].uStep := 40;

g6DControlStatus.sRDICore_Status := '120 | 20 → 40 FB_MoveCyclicPosition DONE Judge If ALL DONE';

END_IF;

30: (*-- MoveInPlane的状态机执行--*)

IF Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Done THEN

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Execute := FALSE;

Ctrl.aSh[Ctrl.i].FB_MoveInPlane(); (*-- 功能块执行 --*)

Ctrl.aSh[Ctrl.i].uStep := 20;

g6DControlStatus.sRDICore_Status := '120 | 30 → 20 FB_MoveInPlane DONE Once';

(*----------------------- 判断数据是否完成 -----------------------*)

IF Ctrl.aSh[Ctrl.i].uPathIndex >= MIN(Ctrl.aSh[Ctrl.i].uPathTotalNum,REQ_PATH_DATA_NUM) THEN

Ctrl.aSh[Ctrl.i].uStep := 40;

g6DControlStatus.sRDICore_Status := '120 | 30 → 40 FB_MoveInPlane DONE Judge If ALL DONE';

END_IF;

40: (*-- 运动结束 --*)

END_CASE;

(*----------------------- 功能块执行 -----------------------*)

FOR Ctrl.i := 0 TO Ctrl.uShuttleNum - 1 DO

IF Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Velocity = 0.0 OR Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Acceleration = 0.0 THEN

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Velocity := 0.5;

Ctrl.aSh[Ctrl.i].FB_MoveInPlane.Parameters.Acceleration := 2;

END_IF;

Ctrl.aSh[Ctrl.i].FB_MoveInPlane(); (*-- 功能块执行 --*)

Ctrl.aSh[Ctrl.i].FB_MoveCyclicPosition();

END_FOR;

代码案例

代码案例