|
<< 点击显示目录 >> 主页 ACOPOS 6D使用手册 > mapp 6D > 功能块 > McAcp6D > Function blocks > MC_BR_MoveInPlane_Acp6D > 代码案例 |
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;
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;
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;