458 lines
19 KiB
C#
458 lines
19 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Drawing;
|
|
using System.Linq;
|
|
using System.Text;
|
|
using AR;
|
|
namespace Project
|
|
{
|
|
public partial class FMain
|
|
{
|
|
public Boolean PICKER_OFF(eWorkPort target, eSMStep cmdIndex)
|
|
{
|
|
UInt16 idx = 1;
|
|
var mv = PUB.Result.vModel;
|
|
var mc = PUB.Result.mModel;
|
|
var funcName = System.Reflection.MethodBase.GetCurrentMethod().Name;
|
|
var seqTime = PUB.sm.seq.GetTime(cmdIndex);
|
|
var msgType = (Class.eStatusMesage)target;
|
|
var item = target == eWorkPort.Left ? PUB.Result.ItemDataL : PUB.Result.ItemDataR;
|
|
// var Jobtype = VAR.STR[eVarString.JOB_TYPE];
|
|
//var ByPassMode = Jobtype == "BP";
|
|
var CVMode = VAR.BOOL[eVarBool.Use_Conveyor];
|
|
|
|
//option check
|
|
var OPT_PrinterOff = PUB.OPT_PRINTEROFF(target);
|
|
var OPT_CameraOff = PUB.OPT_CAMERA(target);
|
|
var OPT_BYPASS = PUB.OPT_BYPASS();
|
|
|
|
var iLockX = PUB.iLock[(int)eAxis.PX_PICK];
|
|
var iLockZ = PUB.iLock[(int)eAxis.PZ_PICK];
|
|
|
|
//####################################################
|
|
//### 인터락 확인
|
|
//####################################################
|
|
Boolean MotMoveX = PUB.mot.IsMotion((int)eAxis.PX_PICK);
|
|
Boolean MotMoveZ = PUB.mot.IsMotion((int)eAxis.PZ_PICK);
|
|
if (iLockX.IsEmpty() == false && MotMoveX)
|
|
{
|
|
var locklistX = MOT.GetActiveLockList(eAxis.PX_PICK, iLockX);
|
|
PUB.mot.MoveStop("ILock(" + string.Join(",", locklistX) + ")", (int)eAxis.PX_PICK);
|
|
}
|
|
|
|
if (iLockZ.IsEmpty() == false && MotMoveZ)
|
|
{
|
|
var locklistZ = MOT.GetActiveLockList(eAxis.PZ_PICK, iLockZ);
|
|
PUB.mot.MoveStop("ILock(" + string.Join(",", locklistZ) + ")", (int)eAxis.PZ_PICK);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//####################################################
|
|
//### 작업시작
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
//컨베이어모드에서는 센서가 감지되면 내려놓지 않게 한다
|
|
if (CVMode)
|
|
{
|
|
if (target == eWorkPort.Left)
|
|
{
|
|
if (DIO.GetIOInput(eDIName.L_CONV1)) return false;
|
|
if (VAR.I32[eVarInt32.LEFT_ITEM_COUNT] > 0) return false;
|
|
}
|
|
else if (target == eWorkPort.Right)
|
|
{
|
|
if (DIO.GetIOInput(eDIName.R_CONV1)) return false;
|
|
if (VAR.I32[eVarInt32.RIGT_ITEM_COUNT] > 0) return false;
|
|
}
|
|
PUB.log.Add($"Conveyor is empty in CV MODE");
|
|
}
|
|
|
|
PUB.log.Add($"[{target}] Picker OFF work started");
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### Y축안전확인
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
//PUB.log.Add($"[{target}] 프린터 Y축 위치 확인");
|
|
var Pos = target == eWorkPort.Left ? MOT.GetLMPos(eLMLoc.READY) : MOT.GetRMPos(eRMLoc.READY);
|
|
if (MOT.getPositionMatch(Pos) == false)
|
|
{
|
|
PUB.Result.SetResultMessage(eResult.OPERATION, eECode.POSITION_ERROR, eNextStep.PAUSE, "Printer Y-axis is not at ready position");
|
|
return false;
|
|
}
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### OFF 위치로 이동
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
if (iLockX.IsEmpty() == false) return false;
|
|
sPositionData PosX;
|
|
if (PUB.flag.get(eVarBool.FG_RUN_LEFT))
|
|
{
|
|
PosX = MOT.GetPXPos(ePXLoc.PICKOFFL);
|
|
}
|
|
else
|
|
{
|
|
PosX = MOT.GetPXPos(ePXLoc.PICKOFFR);
|
|
}
|
|
if (MOT.CheckMotionPos(seqTime, PosX, funcName) == false) return false;
|
|
|
|
//컨베이어모드라면 컨베이어를 멈추게한다
|
|
if (CVMode)
|
|
{
|
|
if (target == eWorkPort.Left)
|
|
PUB.iLockCVL.set((int)eILockCV.BUSY, true, funcName);
|
|
else
|
|
PUB.iLockCVR.set((int)eILockCV.BUSY, true, funcName);
|
|
}
|
|
else
|
|
{
|
|
if (target == eWorkPort.Left)
|
|
PUB.iLockCVL.set((int)eILockCV.BUSY, false, funcName);
|
|
else
|
|
PUB.iLockCVR.set((int)eILockCV.BUSY, false, funcName);
|
|
}
|
|
|
|
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
////####################################################
|
|
////### 프린트부착위치가 상/하가 아니라면 추가적인 회전이 필요하다 - 210207
|
|
////####################################################
|
|
//if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
//{
|
|
// var vData = item.VisionData;
|
|
// if (vData.PrintPositionData == "4") // 왼쪽찍기
|
|
// {
|
|
// if (vData.ReelSize == eCartSize.Inch7)
|
|
// {
|
|
// vData.NeedCylinderForward = false;
|
|
// double targerPos = 0.0;
|
|
// if (target == eWorkShuttle.Left) //left printer
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 90; //90도 회전한다
|
|
// }
|
|
// else
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 90;
|
|
// }
|
|
|
|
// //if (vData.ApplyAngle > 180.0) targerPos = vData.ApplyAngle - 180.0;
|
|
// //else targerPos = vData.ApplyAngle + 180.0;
|
|
|
|
// //추가보정
|
|
// if (targerPos > 360.0) targerPos = targerPos - 360.0;
|
|
// if (targerPos < -360.0) targerPos = targerPos + 360.0;
|
|
|
|
// //지정값대로 추가 회전을 한다
|
|
// var basePosInfo = MOT.GetPTPos(ePTLoc.READY);
|
|
// basePosInfo.Position = targerPos;
|
|
// if (MOT.CheckMotionPos(seqTime, basePosInfo, funcName) == false) return false;
|
|
// }
|
|
// else
|
|
// {
|
|
// //13inch
|
|
// vData.NeedCylinderForward = true;
|
|
// double targerPos = 0.0;
|
|
// if (target == eWorkShuttle.Left) //left printer
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 180; //90도 회전한다
|
|
// }
|
|
// else
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 0;
|
|
// }
|
|
|
|
// //if (vData.ApplyAngle > 180.0) targerPos = vData.ApplyAngle - 180.0;
|
|
// //else targerPos = vData.ApplyAngle + 180.0;
|
|
|
|
// //추가보정
|
|
// if (targerPos > 360.0) targerPos = targerPos - 360.0;
|
|
// if (targerPos < -360.0) targerPos = targerPos + 360.0;
|
|
|
|
// //지정값대로 추가 회전을 한다
|
|
// var basePosInfo = MOT.GetPTPos(ePTLoc.READY);
|
|
// basePosInfo.Position = targerPos;
|
|
// if (MOT.CheckMotionPos(seqTime, basePosInfo, funcName) == false) return false;
|
|
// }
|
|
|
|
// }
|
|
// else if (vData.PrintPositionData == "6") //오른쪽찍기
|
|
// {
|
|
// if (vData.ReelSize == eCartSize.Inch7)
|
|
// {
|
|
// vData.NeedCylinderForward = false;
|
|
// double targerPos = 0.0;
|
|
// if (target == eWorkShuttle.Left) //left printer
|
|
// {
|
|
// targerPos = vData.ApplyAngle - 90; //90도 회전한다
|
|
// }
|
|
// else
|
|
// {
|
|
// targerPos = vData.ApplyAngle - 90;
|
|
// }
|
|
|
|
// //if (vData.ApplyAngle > 180.0) targerPos = vData.ApplyAngle - 180.0;
|
|
// //else targerPos = vData.ApplyAngle + 180.0;
|
|
|
|
// //추가보정
|
|
// if (targerPos > 360.0) targerPos = targerPos - 360.0;
|
|
// if (targerPos < -360.0) targerPos = targerPos + 360.0;
|
|
|
|
// //지정값대로 추가 회전을 한다
|
|
// var basePosInfo = MOT.GetPTPos(ePTLoc.READY);
|
|
// basePosInfo.Position = targerPos;
|
|
// if (MOT.CheckMotionPos(seqTime, basePosInfo, funcName) == false) return false;
|
|
// }
|
|
// else
|
|
// {
|
|
// //13inch
|
|
// vData.NeedCylinderForward = true;
|
|
// double targerPos = 0.0;
|
|
// if (target == eWorkShuttle.Left) //left printer
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 0; //90도 회전한다
|
|
// }
|
|
// else
|
|
// {
|
|
// targerPos = vData.ApplyAngle + 180;
|
|
// }
|
|
|
|
// //if (vData.ApplyAngle > 180.0) targerPos = vData.ApplyAngle - 180.0;
|
|
// //else targerPos = vData.ApplyAngle + 180.0;
|
|
|
|
// //추가보정
|
|
// if (targerPos > 360.0) targerPos = targerPos - 360.0;
|
|
// if (targerPos < -360.0) targerPos = targerPos + 360.0;
|
|
|
|
// //지정값대로 추가 회전을 한다
|
|
// var basePosInfo = MOT.GetPTPos(ePTLoc.READY);
|
|
// basePosInfo.Position = targerPos;
|
|
// if (MOT.CheckMotionPos(seqTime, basePosInfo, funcName) == false) return false;
|
|
// }
|
|
// }
|
|
// else vData.NeedCylinderForward = false; //상하에 붙이는건 실린더를 원위치해야함
|
|
// PUB.sm.seq.Update(cmdIndex);
|
|
// return false;
|
|
//}
|
|
|
|
//####################################################
|
|
//### 사용자회전적용
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
if (item.VisionData.ApplyOffset == false)
|
|
{
|
|
//적용옵셋
|
|
var OffsetAngle = target == eWorkPort.Left ? AR.SETTING.Data.AngleOffsetL : AR.SETTING.Data.AngleOffsetR;
|
|
if (OffsetAngle != 0f)
|
|
{
|
|
if (OPT_BYPASS == true || OPT_PrinterOff)
|
|
{
|
|
PUB.log.AddAT($"Additional rotation not performed due to bypass");
|
|
}
|
|
else
|
|
{
|
|
//모터회전
|
|
//var thpos = MOT.GetPTPos(ePTLoc.READY); //적용할 속도값용
|
|
//MOT.Move(eAxis.Z_THETA, OffsetAngle, thpos.Speed, thpos.Acc, true);
|
|
MOT.Rotation(OffsetAngle, funcName);
|
|
|
|
double theta = 0;
|
|
if (target == eWorkPort.Left) theta = VAR.DBL[(int)eVarDBL.ThetaPositionL];
|
|
else theta = VAR.DBL[(int)eVarDBL.ThetaPositionR];
|
|
|
|
//값을 누적시켜준다
|
|
item.VisionData.ApplyAngle += OffsetAngle;
|
|
|
|
if (target == eWorkPort.Left) VAR.DBL[(int)eVarDBL.ThetaPositionL] = theta + OffsetAngle;
|
|
else VAR.DBL[(int)eVarDBL.ThetaPositionR] = theta + OffsetAngle;
|
|
|
|
if (AR.SETTING.Data.Log_Debug)
|
|
PUB.logDbg.Add($"[{target}] Motor additional rotation: {OffsetAngle}");
|
|
}
|
|
|
|
}
|
|
item.VisionData.ApplyOffset = true;
|
|
}
|
|
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### 회전축 완료 확인
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
if (OPT_BYPASS == false && OPT_PrinterOff == false)
|
|
{
|
|
var Theta = target == eWorkPort.Left ? VAR.DBL[(int)eVarDBL.ThetaPositionL] : VAR.DBL[(int)eVarDBL.ThetaPositionR];
|
|
var Pos = MOT.GetPTPos(ePTLoc.READY);
|
|
Pos.Position = Theta;
|
|
if (MOT.CheckMotionPos(seqTime, Pos, funcName, false) == false) return false;
|
|
PUB.log.Add($"[{target}] Rotation axis verification complete Position: {Theta}, Motor value: {PUB.mot.GetActPos((int)eAxis.Z_THETA)} => Position initialized");
|
|
PUB.mot.ClearPosition((int)eAxis.Z_THETA);
|
|
}
|
|
|
|
|
|
|
|
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### 포트상태확인
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
// if (iLockX.IsEmpty() == false) return false;
|
|
if (iLockZ.IsEmpty() == false) return false;
|
|
var PortRDY = target == eWorkPort.Left ? PUB.flag.get(eVarBool.FG_RDY_PORT_PL) : PUB.flag.get(eVarBool.FG_RDY_PORT_PR);
|
|
if (PortRDY == false) return false;
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### Z축을 내린다.
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
// if (iLockX.IsEmpty() == false) return false;
|
|
if (iLockZ.IsEmpty() == false) return false;
|
|
|
|
//컨베어모드라면 센서에 감지되는 것이 없어야 한다
|
|
if (OPT_BYPASS == false && VAR.BOOL[eVarBool.Use_Conveyor])
|
|
{
|
|
if (target == eWorkPort.Left)
|
|
{
|
|
//컨베이어가 움직이고 있다면 단순 대기한다
|
|
if (DIO.GetIOOutput(eDOName.LEFT_CONV) == false)
|
|
{
|
|
//놓일위치에서 센서가 감지되었다
|
|
if (DIO.GetIOInput(eDIName.L_CONV1))
|
|
{
|
|
var ssname = $"X{eDIName.L_CONV1}";
|
|
PUB.Result.SetResultMessage(eResult.OPERATION, eECode.LCONVER_REEL_DECTECT_IN, eNextStep.PAUSE, ssname);
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (seqTime.TotalSeconds > 10)
|
|
{
|
|
var ssname = $"Y{eDOName.LEFT_CONV}";
|
|
PUB.Result.SetResultMessage(eResult.OPERATION, eECode.LCONVER_MOVING, eNextStep.PAUSE, ssname);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
//컨베이어가 움직이고 있다면 단순 대기한다
|
|
if (DIO.GetIOOutput(eDOName.RIGHT_CONV) == false)
|
|
{
|
|
//놓일위치에서 센서가 감지되었다
|
|
if (DIO.GetIOInput(eDIName.R_CONV1))
|
|
{
|
|
var ssname = $"X{eDIName.R_CONV1}";
|
|
PUB.Result.SetResultMessage(eResult.OPERATION, eECode.RCONVER_REEL_DECTECT_IN, eNextStep.PAUSE, ssname);
|
|
return false;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (seqTime.TotalSeconds > 10)
|
|
{
|
|
var ssname = $"Y{eDOName.RIGHT_CONV}";
|
|
PUB.Result.SetResultMessage(eResult.OPERATION, eECode.RCONVER_MOVING, eNextStep.PAUSE, ssname);
|
|
}
|
|
return false;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
|
|
var fg = target == eWorkPort.Left ? eVarBool.FG_RUN_LEFT : eVarBool.FG_RUN_RIGHT;
|
|
var Loc = target == eWorkPort.Left ? ePZLoc.PICKOFFL : ePZLoc.PICKOFFR;
|
|
var Pos = MOT.GetPZPos(Loc);
|
|
if (MOT.CheckMotionPos(seqTime, Pos, funcName) == false) return false;
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
|
|
//####################################################
|
|
//### 진공파기
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
DIO.SetPickerVac(false);
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### Z축을 올린다.
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
// if (iLockX.IsEmpty() == false) return false;
|
|
if (iLockZ.IsEmpty() == false) return false;
|
|
var Pos = MOT.GetPZPos(ePZLoc.READY);
|
|
if (MOT.CheckMotionPos(seqTime, Pos, funcName) == false) return false;
|
|
PUB.flag.set(eVarBool.FG_PK_ITEMON, false, funcName);
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//###
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
IncCount(1, 1);
|
|
if (target == eWorkPort.Left)
|
|
{
|
|
if (AR.SETTING.Data.Disable_PortL == false && AR.SETTING.Data.Disable_Left == false)
|
|
{
|
|
hmi1.arVar_Port[0].AlignReset();
|
|
}
|
|
|
|
}
|
|
else
|
|
{
|
|
if (AR.SETTING.Data.Disable_PortR == false && AR.SETTING.Data.Disable_Right == false)
|
|
{
|
|
hmi1.arVar_Port[2].AlignReset();
|
|
}
|
|
}
|
|
VAR.I32[eVarInt32.PickOfCount] += 1;
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
}
|
|
} |