278 lines
12 KiB
C#
278 lines
12 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_ON(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 iLockX = PUB.iLock[(int)eAxis.PX_PICK];
|
|
var iLockZ = PUB.iLock[(int)eAxis.PZ_PICK];
|
|
|
|
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();
|
|
|
|
//####################################################
|
|
//### 인터락 확인
|
|
//####################################################
|
|
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++)
|
|
{
|
|
PUB.log.Add($"[{target}] Picker ON work started");
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### ON 위치로 이동
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
if (iLockX.IsEmpty() == false) return false;
|
|
if (iLockZ.IsEmpty() == false) return false;
|
|
var Pos = MOT.GetPXPos(ePXLoc.PICKON);
|
|
var PosZ = MOT.GetPZPos(ePZLoc.READY);
|
|
if (MOT.CheckMotionPos(seqTime, PosZ, funcName) == false) return false;
|
|
if (MOT.CheckMotionPos(seqTime, Pos, funcName) == false) return false;
|
|
PUB.log.Add($"[{target}] Picker ON X,Y ready position confirmed");
|
|
DIO.SetPickerVac(true);
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### Z축을 내린다.
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
var PortRDY = PUB.flag.get(eVarBool.FG_RDY_PORT_PC);
|
|
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;
|
|
var Pos = MOT.GetPZPos(ePZLoc.PICKON);
|
|
var PosMatch = MOT.getPositionMatch(Pos);
|
|
//var PortRDY = PUB.flag.get(eVarBool.RDY_PORT_PC);
|
|
//if (PosMatch==false ) return false;
|
|
|
|
if (MOT.CheckMotionPos(seqTime, Pos, funcName) == false) return false;
|
|
VAR.I32[eVarInt32.PickOnCount] += 1;
|
|
PUB.flag.set(eVarBool.FG_PK_ITEMON, true, funcName);
|
|
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, true, funcName);
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//### 프린트부착위치가 상/하가 아니라면 추가적인 회전이 필요하다 - 210207
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
var vData = target == eWorkPort.Left ? PUB.Result.ItemDataL.VisionData : PUB.Result.ItemDataR.VisionData;
|
|
if (vData.PrintPositionData == "4") // 왼쪽찍기
|
|
{
|
|
if (vData.ReelSize == eCartSize.Inch7)
|
|
{
|
|
vData.NeedCylinderForward = false;
|
|
double targerPos = 0.0;
|
|
if (target == eWorkPort.Left) //left printer
|
|
{
|
|
vData.PositionAngle = -90; //90도 회전한다
|
|
}
|
|
else
|
|
{
|
|
vData.PositionAngle = -90;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//13inch
|
|
vData.NeedCylinderForward = true;
|
|
if (target == eWorkPort.Left) //left printer
|
|
{
|
|
vData.PositionAngle = -180;
|
|
}
|
|
else
|
|
{
|
|
vData.PositionAngle = 0;
|
|
}
|
|
}
|
|
}
|
|
else if (vData.PrintPositionData == "6") //오른쪽찍기
|
|
{
|
|
if (vData.ReelSize == eCartSize.Inch7)
|
|
{
|
|
vData.NeedCylinderForward = false;
|
|
if (target == eWorkPort.Left) //left printer
|
|
{
|
|
vData.PositionAngle = 90; //90도 회전한다
|
|
}
|
|
else
|
|
{
|
|
vData.PositionAngle = 90; //90도 회전한다
|
|
}
|
|
}
|
|
else
|
|
{
|
|
//13inch
|
|
vData.NeedCylinderForward = true;
|
|
double targerPos = 0.0;
|
|
if (target == eWorkPort.Left) //left printer
|
|
{
|
|
vData.PositionAngle = 0;
|
|
}
|
|
else
|
|
{
|
|
vData.PositionAngle = 180;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
vData.NeedCylinderForward = false; //상하에 붙이는건 실린더를 원위치해야함
|
|
vData.PositionAngle = 0;
|
|
}
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
|
|
//####################################################
|
|
//### 바코드에의한 자동 회전
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
//속도값만 취한다
|
|
var thpos = MOT.GetPTPos(ePTLoc.READY);
|
|
|
|
// Baseangle 값으로 변경 210126 - qr의 각도가잇다면 그것이 기준이됨
|
|
double baseAngle = 0;
|
|
string baseSource = "F";
|
|
var vdata = target == eWorkPort.Left ? PUB.Result.ItemDataL.VisionData : PUB.Result.ItemDataR.VisionData;
|
|
if (vdata.BaseAngle(out string msg, out Class.KeyenceBarcodeData bcd))
|
|
{
|
|
baseAngle = bcd.Angle;
|
|
baseSource = bcd.barcodeSource; //230504 각도
|
|
PUB.log.AddI($"[{target}] Angle calculation: {msg}, Barcode: {bcd.Data}, ID: {vdata.RID}"); //210602
|
|
}
|
|
else if (PUB.Result.DryRun)
|
|
{
|
|
PUB.log.AddAT($"[{target}] Angle not applied due to dry run");
|
|
}
|
|
else
|
|
{
|
|
PUB.log.AddE($"[{target}] base ange error : {msg},ID:{vdata.RID}");
|
|
}
|
|
|
|
var addangle = baseSource == "R" ? SETTING.Data.RearBarcodeRotate : SETTING.Data.FrontBarcodeRotate;
|
|
var rotAngle = baseAngle + addangle + vdata.PositionAngle;
|
|
PUB.log.AddI($"[{target}] Source: {baseSource} Rotation value (Base: {baseAngle} + Additional: {addangle} + Vision: {vdata.PositionAngle} = {rotAngle})");
|
|
|
|
int dir = -1;
|
|
while (true)
|
|
{
|
|
if (rotAngle > 360.0)
|
|
rotAngle = rotAngle - 360.0; //한반퀴 더 도는것은 역 처리한다
|
|
else break;
|
|
}
|
|
//rotAngle += 90; //비젼이 90도 돌아가잇으니 다시 적용필요함 //상단이위로가도록 함
|
|
if (rotAngle > 180)
|
|
{
|
|
rotAngle = 360 - rotAngle;
|
|
dir = 1;
|
|
}
|
|
|
|
PUB.log.AddI($"[{target}] Rotation (Final) [{rotAngle} degrees, Direction: {dir}], RID: {vdata.RID}");
|
|
|
|
if (target == eWorkPort.Left)
|
|
PUB.Result.ItemDataL.VisionData.ApplyAngle = rotAngle; //회전각도를 넣는다
|
|
else
|
|
PUB.Result.ItemDataR.VisionData.ApplyAngle = rotAngle; //회전각도를 넣는다
|
|
|
|
var curtheta = PUB.mot.GetActPos((int)eAxis.Z_THETA);
|
|
var newPos = curtheta + (dir * rotAngle);
|
|
PUB.log.Add($"Motor position before rotation: {curtheta}, Target position: {newPos}, Speed: {thpos.Speed}, Acceleration: {thpos.Acc}");
|
|
|
|
if (target == eWorkPort.Left)
|
|
VAR.DBL[(int)eVarDBL.ThetaPositionL] = newPos;
|
|
else
|
|
VAR.DBL[(int)eVarDBL.ThetaPositionR] = newPos;
|
|
|
|
//if (ByPassMode == false)
|
|
//{
|
|
//바이패스에서도 추가 회전을 한다
|
|
MOT.Move(eAxis.Z_THETA, newPos, thpos.Speed, thpos.Acc, false);
|
|
//}
|
|
//else PUB.log.AddAT($"bypass 로 인해 회전을 하지 않습니다");
|
|
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
//####################################################
|
|
//###
|
|
//####################################################
|
|
if (PUB.sm.seq.Get(cmdIndex) == idx++)
|
|
{
|
|
|
|
PUB.sm.seq.Update(cmdIndex);
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
}
|
|
} |