private void WriteCSVLog(BarcodeReaderData barcodeReaderData, string barcodeReaderName)
{
try
{
string csvLog = String.Concat(DateTime.Now.ToString("yyyy/MM/dd HH:mm:ss.fff"));
if (barcodeReaderData != null)
{
csvLog = String.Concat(csvLog, ",", barcodeReaderData.GetDataTime.ToString("yyyy/MM/dd HH:mm:ss.fff"), ",", barcodeReaderName, ",",
barcodeReaderData.Count.ToString(), ",", barcodeReaderData.LocateData.ScanTime, ",",
barcodeReaderData.LocateData.Type.ToString());
if (barcodeReaderData.Barcode1 != null && barcodeReaderData.Barcode2 != null)
{
csvLog = String.Concat(csvLog, ",", barcodeReaderData.Barcode1.ID.ToString(), ",",
barcodeReaderData.Barcode1.ViewPosition.X.ToString(), ",", barcodeReaderData.Barcode1.ViewPosition.Y.ToString(), ",",
barcodeReaderData.Barcode1.MapPosition.X.ToString(), ",", barcodeReaderData.Barcode1.MapPosition.Y.ToString(), ",",
barcodeReaderData.Barcode2.ID.ToString(), ",",
barcodeReaderData.Barcode2.ViewPosition.X.ToString(), ",", barcodeReaderData.Barcode2.ViewPosition.Y.ToString(), ",",
barcodeReaderData.Barcode2.MapPosition.X.ToString(), ",", barcodeReaderData.Barcode2.MapPosition.Y.ToString());
if (barcodeReaderData.LocateData != null)
{
csvLog = String.Concat(csvLog, ",", barcodeReaderData.LocateData.AGVPosition.Position.X.ToString(), ",", barcodeReaderData.LocateData.AGVPosition.Position.Y.ToString(), ",", barcodeReaderData.LocateData.AGVPosition.Angle.ToString());
}
//csvLog = String.Concat(csvLog, ",", barcodeReaderData.TargetCenter.X.ToString(), ",", barcodeReaderData.TargetCenter.Y.ToString(), ",", barcodeReaderData.LocateData.AGVPosition.Angle.ToString());
}
}
logger.LogString(csvLog);
}
catch { }
}
private bool GetReadDataPosition(BarcodeReaderData barcodeReaderData)
{
if (GetBarcodePosition(barcodeReaderData.Barcode1) && GetBarcodePosition(barcodeReaderData.Barcode2))
{
return(barcodeReaderData.Barcode1.LineId == barcodeReaderData.Barcode2.LineId);
}
else
{
return(false);
}
}
public LocateAGVPosition GetLocateAGVPositionByIndex(int index)
{
if (index < config.BarcodeReaderConfigs.Count)
{
BarcodeReaderData temp = barcodeReaderDataList[index];
if (temp == null)
{
return(null);
}
else
{
return(temp.LocateData);
}
}
else
{
return(null);
}
}
private void ComputeMapPosition(int index, BarcodeReaderData barcodeReaderData)
{
double barcodeAngleInView = computeFunction.ComputeAngle(barcodeReaderData.Barcode1.ViewPosition, barcodeReaderData.Barcode2.ViewPosition); // pointer barcode1 to barcode2.
// barcode在reader上面的角度 = barcode - reader.
double barcodeAngleInMap = computeFunction.ComputeAngle(barcodeReaderData.Barcode1.MapPosition, barcodeReaderData.Barcode2.MapPosition); // barcode in Map's angle.
// barcode在Map上的角度 = barcode - Map.
double barcode1ToCenterAngleInView = computeFunction.ComputeAngle(barcodeReaderData.Barcode1.ViewPosition, config.BarcodeReaderConfigs[index].Target);
double barcode1ToCenterAngleInMap = 0;
double agvAngleInMap = 0;
double barcode1ToCenterDistance = Math.Sqrt(Math.Pow((config.BarcodeReaderConfigs[index].Target.X - barcodeReaderData.Barcode1.ViewPosition.X) * config.BarcodeReaderConfigs[index].Change.X, 2) +
Math.Pow((config.BarcodeReaderConfigs[index].Target.Y - barcodeReaderData.Barcode1.ViewPosition.Y) * config.BarcodeReaderConfigs[index].Change.Y, 2));
barcodeAngleInView += config.BarcodeReaderConfigs[index].Offset.Angle;
barcode1ToCenterAngleInView += config.BarcodeReaderConfigs[index].Offset.Angle;
// Map在reader上的角度 = Map -reader = barcodeInViewAngle - barcodeInMapAngle;
agvAngleInMap = barcodeAngleInMap - barcodeAngleInView + config.BarcodeReaderConfigs[index].ReaderSetupAngle;
if (agvAngleInMap > 180)
{
agvAngleInMap -= 360;
}
else if (agvAngleInMap <= -180)
{
agvAngleInMap += 360;
}
barcode1ToCenterAngleInMap = barcodeAngleInMap - barcodeAngleInView + barcode1ToCenterAngleInView;
barcodeReaderData.TargetCenter =
new MapPosition(barcodeReaderData.Barcode1.MapPosition.X + (float)(barcode1ToCenterDistance * Math.Cos(-barcode1ToCenterAngleInMap / 180 * Math.PI)),
barcodeReaderData.Barcode1.MapPosition.Y + (float)(barcode1ToCenterDistance * Math.Sin(-barcode1ToCenterAngleInMap / 180 * Math.PI)));
MapPosition agvPosition = new MapPosition(
barcodeReaderData.TargetCenter.X + (float)(config.BarcodeReaderConfigs[index].ReaderToCenterDistance *
Math.Cos((agvAngleInMap + config.BarcodeReaderConfigs[index].ReaderToCenterDegree + 180) / 180 * Math.PI)),
barcodeReaderData.TargetCenter.Y + (float)(config.BarcodeReaderConfigs[index].ReaderToCenterDistance *
Math.Sin((agvAngleInMap + config.BarcodeReaderConfigs[index].ReaderToCenterDegree + 180) / 180 * Math.PI)));
barcodeReaderData.LocateData = new LocateAGVPosition(agvPosition, agvAngleInMap, barcodeAngleInMap, barcodeReaderData.ScanTime,
barcodeReaderData.GetDataTime, barcodeReaderData.Count, barcodeReaderData.Barcode1.Type, device, DriverConfig.Order);
}
private void PollingThread(object index)
{
string data = "";
uint count = 0;
string errorMessage = "";
int intIndex = (int)index;
int noDataCount = 0;
BarcodeReaderData barcodeReaderData;
Stopwatch timeoutTimer = new Stopwatch();
bool dataNoMath = false;
while (Status != EnumControlStatus.Closing)
{
barcodeReaderData = null;
timeoutTimer.Restart();
if (PoolingOnOff)
{
try
{
if (BarcodeReaderList[intIndex].Error)
{
;
}
else if (BarcodeReaderList[intIndex].ReadBarcode(ref data, config.TimeoutValue, ref errorMessage))
{
barcodeReaderData = new BarcodeReaderData(data, config.BarcodeReaderConfigs[intIndex].BarcodeReaderType, count);
if (barcodeReaderData.DataNotMatch)
{
if (!dataNoMath)
{
WriteLog(3, "", String.Concat("BarcodeReader ID : ", config.BarcodeReaderConfigs[intIndex].ID, " DataNotMath, Data : ", data));
SendAlarmCode(EnumMoveCommandControlErrorCode.LocateDriver_BarcodeMapSystem回傳資料格式錯誤);
dataNoMath = true;
}
}
else
{
if (dataNoMath)
{
WriteLog(3, "", String.Concat("BarcodeReader ID : ", config.BarcodeReaderConfigs[intIndex].ID, " Data cahnge to Math"));
dataNoMath = false;
}
#region Barcode資料正常,計算.
//XChangeTheta55ALL(intIndex, barcodeReaderData);
if (GetReadDataPosition(barcodeReaderData))
{
ComputeMapPosition(intIndex, barcodeReaderData);
if (barcodeReaderData.ScanTime > config.TimeoutValue || noDataCount >= 3)
{
if (barcodeReaderData.LocateData != null)
{
barcodeReaderData.LocateData.Type = EnumAGVPositionType.OnlyRead;
}
}
noDataCount = 0;
}
#endregion
}
}
else
{
noDataCount++;
if (BarcodeReaderList[intIndex].Error)
{
WriteLog(3, "", String.Concat("BarcodeReader ID : ", config.BarcodeReaderConfigs[intIndex].ID, ", Error : ", errorMessage.ToString()));
allNotError = true;
status = EnumControlStatus.Error;
}
}
count++;
}
catch (Exception ex)
{
WriteLog(3, "", String.Concat("BarcodeReader ID : ", config.BarcodeReaderConfigs[intIndex].ID, ", Exception : ", ex.ToString()));
SendAlarmCode(EnumMoveCommandControlErrorCode.LocateDriver_BarcodeMapSystemTriggerException);
}
}
barcodeReaderDataList[intIndex] = barcodeReaderData;
#region 寫入CSV.
if (config.LogMode /*&& barcodeReaderData != null && barcodeReaderData.AGV != null*/)
{
lock (lockObejct)
{
WriteCSVLog(barcodeReaderData, config.BarcodeReaderConfigs[intIndex].ID);
}
}
#endregion
while (timeoutTimer.ElapsedMilliseconds < config.TimeoutValue)
{
Thread.Sleep(1);
}
Thread.Sleep(config.SleepTime);
}
}
private void XChangeTheta55ALL(int index, BarcodeReaderData sr2000ReadData)
{
sr2000ReadData.Barcode1.ViewPosition = XChangeTheta55Single(index, sr2000ReadData.Barcode1.ViewPosition);
sr2000ReadData.Barcode2.ViewPosition = XChangeTheta55Single(index, sr2000ReadData.Barcode2.ViewPosition);
}
