350 lines
13 KiB
C#
350 lines
13 KiB
C#
using Aurora.Api;
|
|
using Aurora.Util.HislogPlus;
|
|
using AuroraLaser;
|
|
using AuroraLaser.Trumpf;
|
|
using Hermes;
|
|
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Linq;
|
|
using System.Net;
|
|
using System.Runtime.Remoting.Contexts;
|
|
using System.Runtime.Remoting.Messaging;
|
|
using System.Text;
|
|
using System.Threading.Tasks;
|
|
|
|
|
|
namespace AuroraOCT.Trumpf
|
|
{
|
|
/// <summary>
|
|
/// Laser에 종속적으로 동작해야해서 제조사 별(컨셉이 다름..) Device를 만들어서 동작하도록 함
|
|
/// TruControl 1000 v4.04 이상 사용 가능
|
|
/// </summary>
|
|
public class OCT : Device
|
|
{
|
|
private Logger _log = new Logger("OCT");
|
|
private Laser _laser;
|
|
private LaserTruFiber _core;
|
|
|
|
private OCTConfig _config;
|
|
|
|
private AngleTracker _tracker = new AngleTracker();
|
|
public int LightPath { get; set; }
|
|
public delegate double getDouble();
|
|
public getDouble GetAXVelocity, GetAXPosition, GetAYVelocity, GetAYPosition;
|
|
public getDouble GetBXVelocity, GetBXPosition, GetBYVelocity, GetBYPosition;
|
|
|
|
public OCT(Laser laser) {
|
|
_laser = laser;
|
|
}
|
|
|
|
protected override int DoActivate(IDeviceConfig config)
|
|
{
|
|
_config = config as OCTConfig;
|
|
_core = _laser.getCore() as LaserTruFiber;
|
|
|
|
if (_laser.IsActivated && _config.IsMonitoringStart){
|
|
SetMonitoringService(UpdatePosition, _config.UpdateInterval);
|
|
StartMonitoringService();
|
|
}
|
|
|
|
return _laser.IsActivated ? E1.OK : E2.ABORTED;
|
|
}
|
|
|
|
protected override int DoDeactivate()
|
|
{
|
|
StopMonitoringService();
|
|
return E1.OK;
|
|
}
|
|
|
|
private void UpdatePosition()
|
|
{
|
|
double XVel, YVel, XPos, YPos;
|
|
if (LightPath == 0) {
|
|
XVel = GetAXVelocity();
|
|
YVel = GetAYVelocity();
|
|
XPos = GetAXPosition();
|
|
YPos = GetAYPosition();
|
|
} else {
|
|
XVel = GetBXVelocity();
|
|
YVel = GetBYVelocity();
|
|
XPos = -GetBXPosition();
|
|
YPos = -GetBYPosition();
|
|
}
|
|
|
|
// X,Y 속도의 스칼라량 (벡터의 크기를 전달)
|
|
var velocityVector = Math.Sqrt(XVel * XVel + YVel * YVel);
|
|
var value = velocityVector < 0.001 ? 0.001 : velocityVector;
|
|
SetPositionX(value);
|
|
|
|
// feed direction (각 변화량)
|
|
var result = _tracker.UpdatePosition(XPos, YPos);
|
|
if (result.HasValue) {
|
|
value = result.Value.AngleDeg < 0.001 ? 0.001 : result.Value.AngleDeg;
|
|
SetPositionY(value);
|
|
//Debug.WriteLine($"velocity vector : {velocityVector} / AngleDeg : {result.Value.AngleDeg}");
|
|
}
|
|
}
|
|
public double GetPositionX() {
|
|
var word1 = _core.GetWriteInputWord(_config.XPosIW_2W);
|
|
var word2 = _core.GetWriteInputWord(_config.XPosIW_2W + 1);
|
|
UInt32 raw = ((UInt32)(ushort)word2 << 16) | (UInt32)(ushort)word1;
|
|
return raw / 1000.0;
|
|
}
|
|
|
|
public double GetPositionY()
|
|
{
|
|
var word1 = _core.GetWriteInputWord(_config.YPosIW_2W);
|
|
var word2 = _core.GetWriteInputWord(_config.YPosIW_2W + 1);
|
|
UInt32 raw = ((UInt32)(ushort)word2 << 16) | (UInt32)(ushort)word1;
|
|
return raw / 1000.0;
|
|
}
|
|
public void SetInquiry1(bool on) => _core.SetData(csWriteNodeNo.INQUIRY_1, on ? 1 : 0);
|
|
public void SetPositionX(double pos) => _core.SetDWord(_config.XPosIW_2W, (int)(pos * 1000));
|
|
public void SetPositionY(double pos) => _core.SetDWord(_config.YPosIW_2W, (int)(pos * 1000));
|
|
|
|
public void SetWorkPieceSerialNo(string SerialNo)
|
|
{
|
|
short[] words = ToWordsLowFirst(SerialNo, 4);
|
|
ulong wordsTolong = WordsToUlong_LswFirst(words);
|
|
_core.SetMultiWord(_config.WpSerialNoIW_4W, 4, wordsTolong);
|
|
}
|
|
|
|
public void SetWorkPieceID(string id)
|
|
{
|
|
short[] words = ToWordsLowFirst(id, 4);
|
|
ulong wordsTolong = WordsToUlong_LswFirst(words);
|
|
_core.SetMultiWord(_config.WpIdIW_4W, 4, wordsTolong);
|
|
}
|
|
|
|
public void SetOctReady(bool preTrigger, bool inTrigger, bool posGen)
|
|
{
|
|
int CurrentWordValue = 0;
|
|
CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.PreTrigger, preTrigger);
|
|
CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.InTrigger, inTrigger);
|
|
CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.PosGen, posGen);
|
|
|
|
try {
|
|
_core.SetData(_config.VLCKcontrolIW, CurrentWordValue);
|
|
} catch (Exception ex) {
|
|
_log.D($"SetOctReady Fails; {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public void SetPreTrigger(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PreTrigger, on);
|
|
public bool IsPreTrigger() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PreTrigger);
|
|
|
|
public void SetInTrigger(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.InTrigger, on);
|
|
public bool IsInTrigger() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.InTrigger);
|
|
|
|
public void SetPosGen(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PosGen, on);
|
|
public bool IsPosGen() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PosGen);
|
|
|
|
public void SetLightPath(int no) => _core.SetData(csWriteNodeNo.LIGHT_PATH_NO, no);
|
|
|
|
public bool IsSensoricReady() => GetOctOutputBit(eOctControlOutputBit.SensoricReady);
|
|
public bool IsMeasureActive() => GetOctOutputBit(eOctControlOutputBit.MeasureActive);
|
|
public bool IsSeamOk() => GetOctOutputBit(eOctControlOutputBit.SeamOk);
|
|
public bool IsSeamNok() => GetOctOutputBit(eOctControlOutputBit.SeamNok);
|
|
public bool IsProcedureFaultSeam() => GetOctOutputBit(eOctControlOutputBit.ProcedureFaultSeam);
|
|
public bool IsPartOk() => GetOctOutputBit(eOctControlOutputBit.PartOk);
|
|
public bool IsPartNok() => GetOctOutputBit(eOctControlOutputBit.PartNok);
|
|
public bool IsProcedureFaultPart() => GetOctOutputBit(eOctControlOutputBit.ProcedureFaultPart);
|
|
|
|
public (eOctControlOutputBit result, bool success) GetSeamResult()
|
|
{
|
|
(eOctControlOutputBit result, bool success) res;
|
|
bool[] results = new bool[3];
|
|
results[0] = IsSeamOk();
|
|
results[1] = IsSeamNok();
|
|
results[2] = IsProcedureFaultSeam();
|
|
|
|
if(results[0])
|
|
res.result = eOctControlOutputBit.SeamOk;
|
|
else if (results[1])
|
|
res.result = eOctControlOutputBit.SeamNok;
|
|
else
|
|
res.result = eOctControlOutputBit.ProcedureFaultSeam;
|
|
|
|
res.success = results.Select(ok => ok ? 1 : 0).Sum() > 0;
|
|
return res;
|
|
}
|
|
|
|
public (eOctControlOutputBit result, bool success) GetPartResult()
|
|
{
|
|
(eOctControlOutputBit result, bool success) res;
|
|
bool[] results = new bool[3];
|
|
results[0] = IsPartOk();
|
|
results[1] = IsPartNok();
|
|
results[2] = IsProcedureFaultPart();
|
|
|
|
if (results[0])
|
|
res.result = eOctControlOutputBit.PartOk;
|
|
else if (results[1])
|
|
res.result = eOctControlOutputBit.PartNok;
|
|
else
|
|
res.result = eOctControlOutputBit.ProcedureFaultPart;
|
|
|
|
res.success = results.Select(ok => ok ? 1 : 0).Sum() > 0;
|
|
return res;
|
|
}
|
|
|
|
#region OCT 관련 비트 매핑
|
|
public enum eOctControlOutputBit : int
|
|
{
|
|
SensoricReady = 0,
|
|
MeasureActive = 1,
|
|
SeamOk = 2,
|
|
SeamNok = 3,
|
|
PartOk = 4,
|
|
PartNok = 5,
|
|
ProcedureFaultSeam = 8,
|
|
ProcedureFaultPart = 9,
|
|
}
|
|
|
|
public enum eOctControlInputBit
|
|
{
|
|
PreTrigger =0,
|
|
InTrigger =1,
|
|
PosGen = 15
|
|
}
|
|
|
|
private bool GetOctOutputBit(eOctControlOutputBit no) {
|
|
if (!Enum.IsDefined(typeof(eOctControlOutputBit), no)) return false;
|
|
|
|
int temp = _core.ReadNode[_config.OutputWord].n16Ret;
|
|
|
|
return (temp & (1 << (int)no)) != 0;
|
|
}
|
|
|
|
private int SetBit(int value, int bit, bool flag)
|
|
{
|
|
if (flag)
|
|
return value | (1 << (int)bit);
|
|
else
|
|
return value & ~(1 << (int)bit);
|
|
}
|
|
|
|
private bool SetOctOutputBit(int wordNo, int bitNo, bool on)
|
|
{
|
|
var CurrentWordValue = _core.GetWriteInputWord(wordNo);
|
|
CurrentWordValue = SetBit(CurrentWordValue, bitNo, on);
|
|
|
|
try {
|
|
_core.SetData(wordNo, CurrentWordValue);
|
|
return true;
|
|
} catch (Exception ex) {
|
|
_log.D($"SetOctInputBits Fails; {ex.Message}");
|
|
return false;
|
|
}
|
|
}
|
|
|
|
private bool GetOctOutputBit(int wordNo, int bitNo)
|
|
{
|
|
var CurrentWordValue = _core.GetWriteInputWord(wordNo);
|
|
return (CurrentWordValue & (1 << (int)bitNo)) != 0;
|
|
}
|
|
//////// 시퀸스 관련 정리
|
|
/// TruFiber의 LaserOn으로 Laser Ready 갈 수 있음.
|
|
/// !! 주의. Program No는 0일 수 있으므로, 유동적으로 변경 가능하도록 수정하여야 할 듯 함.
|
|
/// 이 후에는 우리가 시퀸스에서 제어
|
|
/// OCT -> PC : PC가 값 읽어야 함. PC -> OCT : PC에서 값을 써주어야 함.
|
|
/// 0. OCT -> PC : PC는 LASER_READY 확인
|
|
/// 1. PC -> OCT : PSTART_STATICAL ON
|
|
/// 2. OCT -> PC : PC는 LASER_READY OFF 확인 & SENSORIC_READY ON 확인
|
|
/// 3. PC -> OCT : XPOS,YPOS,POS_GEN 값 변경
|
|
/// 4. OCT -> PC : Reach In Pos 어떻게 확인하지 ?
|
|
/// 5. PC -> OCT : PRE_TRIGGER와 IN_TRIGGER ON. 동시에 킬 것 !!
|
|
/// ##### 레이저 출사 시작
|
|
/// 6. OCT -> PC : MEASUREMENT_ACTIVE ON 확인 (OCT 문제 없는 상태임.)
|
|
/// ##### 레이저 출사 종료
|
|
/// 7. OCT -> PC : MEASUREMENT_ACTIVE OFF 확인 (OCT 측정 종료된 상태임.)
|
|
/// 8. PC -> OCT : (5)에서 써준 값 OFF
|
|
/// 9. OCT -> PC : SEAM_OK or NON_OK or PROCEDURE_FAULT_SEAM ON 확인
|
|
/// 10. OCT -> PC : PROG_COMPLETED ON 확인
|
|
/// 11. PC -> OCT : PSTART_STATICAL OFF
|
|
/// 12. OCT -> PC : Part 관련 값 확인
|
|
#endregion
|
|
|
|
public short[] ToWordsLowFirst(string text, int wordCount = 4)
|
|
{
|
|
// 4워드면 최대 8바이트(문자 8개)까지
|
|
int maxBytes = wordCount * 2;
|
|
byte[] bytes = Encoding.ASCII.GetBytes(text);
|
|
|
|
if (bytes.Length > maxBytes)
|
|
return null;
|
|
|
|
var result = new short[wordCount];
|
|
|
|
int bi = 0;
|
|
for (int wi = 0; wi < wordCount; wi++) {
|
|
byte hi = (bi < bytes.Length) ? bytes[bi++] : (byte)0x00;
|
|
byte lo = (bi < bytes.Length) ? bytes[bi++] : (byte)0x00;
|
|
|
|
ushort word = (ushort)((hi << 8) | lo); // 예: 'W','P' => 0x5750
|
|
result[wi] = unchecked((short)word); // short에 담아 리턴
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
public ulong WordsToUlong_LswFirst(short[] words)
|
|
{
|
|
if (words == null)
|
|
return 0;
|
|
|
|
if (words.Length != 4)
|
|
return 0;
|
|
|
|
ulong v = 0;
|
|
for (int i = 0; i < 4; i++) {
|
|
v |= ((ulong)(ushort)words[i]) << (16 * i);
|
|
}
|
|
return v;
|
|
}
|
|
|
|
public class AngleTracker
|
|
{
|
|
private double? _prevAngleDeg = null;
|
|
private (double X, double Y)? _prevPosition = null;
|
|
|
|
/// 현재 위치를 기준으로 진행 각도(degree)와 이전 위치 대비 각도 변화량을 계산
|
|
public (double AngleDeg, double DeltaAngleDeg)? UpdatePosition(double currX, double currY)
|
|
{
|
|
if (!_prevPosition.HasValue) {
|
|
_prevPosition = (currX, currY);
|
|
return null;
|
|
}
|
|
|
|
var (prevX, prevY) = _prevPosition.Value;
|
|
|
|
double dx = currX - prevX;
|
|
double dy = currY - prevY;
|
|
|
|
// 각도 계산
|
|
double angleRad = Math.Atan2(dy, dx);
|
|
double angleDeg = angleRad * (180.0 / Math.PI);
|
|
|
|
// 정규화
|
|
double angleDeg360 = (angleDeg + 360) % 360;
|
|
double deltaAngle = 0;
|
|
|
|
if (_prevAngleDeg.HasValue) {
|
|
deltaAngle = angleDeg - _prevAngleDeg.Value;
|
|
|
|
// -180 ~ 180 사이로 정규화
|
|
while (deltaAngle > 180) deltaAngle -= 360;
|
|
while (deltaAngle < -180) deltaAngle += 360;
|
|
}
|
|
|
|
// 상태 업데이트
|
|
_prevPosition = (currX, currY);
|
|
_prevAngleDeg = angleDeg;
|
|
|
|
return (angleDeg360, deltaAngle);
|
|
}
|
|
}
|
|
}
|
|
}
|