/// <summary>
/// 获取2个RunnableModule之间的同步坐标转换器
/// </summary>
/// <param name="markCmd"></param>
/// <returns></returns>
public CoordinateTransformer GetSimul(RunnableModule mainModule, RunnableModule simulModule)
{
CoordinateTransformer transformer = new CoordinateTransformer();
//默认使用两个pattern的原点进行纠偏，（有Mark则使用mark纠偏）
transformer.SetMarkPoint(mainModule.Origin, simulModule.Origin);
//MarkInfo中没有对应Mark指令，说明对应pattern没有添加Mark
if (!markInfoMap.ContainsKey(mainModule))
{
return(transformer);
}
Dictionary <MarkCmd, PointD> marks = markInfoMap[mainModule];
if (marks.Count <= 0)
{
return(transformer);
}
int count = 0;
PointD module1Mark1 = new PointD(), module1Mark2 = new PointD(), module2Mark1 = new PointD(), module2Mark2 = new PointD();
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
module1Mark1.CopyFrom(marks[markCmd]);
}
else if (count == 1)
{
module1Mark2.CopyFrom(marks[markCmd]);
// 脚本语法上限制了最多添加两个Mark点，此处可直接跳出循环
break;
}
count++;
}
marks = markInfoMap[simulModule];
count = 0;
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
module2Mark1.CopyFrom(marks[markCmd]);
}
else if (count == 1)
{
module2Mark2.CopyFrom(marks[markCmd]);
// 脚本语法上限制了最多添加两个Mark点，此处可直接跳出循环
break;
}
count++;
}
if (count == 1)
{
transformer.SetMarkPoint(module1Mark1, module1Mark2, module2Mark1, module2Mark2);
}
else
{
transformer.SetMarkPoint(module1Mark1, module2Mark1);
}
return(transformer);
}
/// <summary>
/// Get the distence between the mark1 of the two runnableModules
/// if runnable don't have any marks,return the distence between the origin of the two runnableModules
/// </summary>
/// <param name="mainModule"></param>
/// <param name="simulModule"></param>
/// <returns></returns>
public VectorD GetRunnableDistence(RunnableModule mainModule, RunnableModule simulModule)
{
if (!markInfoMap.ContainsKey(mainModule) || !markInfoMap.ContainsKey(mainModule))
{
return(mainModule.Origin - simulModule.Origin);
}
Dictionary <MarkCmd, PointD> marks = markInfoMap[mainModule];
if (marks.Count < 1)
{
return(mainModule.Origin - simulModule.Origin);
}
else
{
PointD module1Mark1 = new PointD(); //主mark在机械坐标系中坐标
PointD module2Mark1 = new PointD(); //副阀mark在机械坐标系中坐标
int count = 0;
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
module1Mark1.CopyFrom(marks[markCmd]);
break;
}
}
count = 0;
marks = markInfoMap[simulModule];
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
module2Mark1.CopyFrom(marks[markCmd]);
break;
}
}
return(module1Mark1 - module2Mark1);
}
}
/// <summary>
/// 当飞拍所有的mark点命令执行完后，需要生成transMap
/// </summary>
public void OnAllMarkCmdsExecuted()
{
transMap.Clear();
PointD mark1 = new PointD(), real1 = new PointD(), mark2 = new PointD(), real2 = new PointD();
double markRotation = 0, realRotation = 0;
foreach (RunnableModule module in markInfoMap.Keys)
{
if (module.Mode == ModuleMode.SkipMode)
{
continue;
}
Dictionary <MarkCmd, PointD> marks = markInfoMap[module];
MarkCmd singleMark = null;
if (marks.Count <= 0)
{
continue;
}
int count = 0;
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
singleMark = markCmd;
if (singleMark.ModelFindPrm.IsUnStandard)
{
//非标
var structure = module.CommandsModule.program.ModuleStructure;
PointD p = new PointD(singleMark.ModelFindPrm.ReferenceX, singleMark.ModelFindPrm.ReferenceY);
PointD p2 = new PointD(singleMark.ModelFindPrm.ReferenceX2, singleMark.ModelFindPrm.ReferenceY2);
mark1.CopyFrom(structure.ToMachine(module, p));
real1.CopyFrom(marks[markCmd]);
// 飞拍Mark点2取值
if (singleMark.ModelFindPrm.UnStandardType == 0)
{
markRotation = singleMark.ModelFindPrm.ReferenceA;
realRotation = asvMarkAngles[markCmd];
}
else
{
mark2.CopyFrom(structure.ToMachine(module, p2));
real2.CopyFrom(asvMarkPoint2s[markCmd]);
}
}
else
{
mark1.CopyFrom(markCmd.Position);
real1.CopyFrom(marks[markCmd]);
}
}
else if (count == 1)
{
mark2.CopyFrom(markCmd.Position);
real2.CopyFrom(marks[markCmd]);
// 脚本语法上限制了最多添加两个Mark点，此处可直接跳出循环
break;
}
count++;
}
CoordinateTransformer transformer = new CoordinateTransformer();
if (marks.Count == 1)
{
Log.Dprint(TAG, "runnable mark count is 1, standard pos : " + mark1 + ", real pos : " + real1);
if (singleMark.ModelFindPrm.IsUnStandard)
{
if (singleMark.ModelFindPrm.UnStandardType == 0)
{
transformer.SetMarkPoint(mark1, markRotation, real1, realRotation);
}
else
{
transformer.SetMarkPoint(mark1, mark2, real1, real2);
}
}
else
{
transformer.SetMarkPoint(mark1, real1);
}
}
else
{
Log.Dprint(TAG, "runnable mark count is 2, standard pos1 : " + mark1 + ", real pos1 : " + real1
+ ", standard pos2 : " + mark2 + ", real pos2 : " + real2);
transformer.SetMarkPoint(mark1, mark2, real1, real2);
}
transMap.Add(module, transformer);
}
}
/// <summary>
/// 调用前提，当前RunnableModule的Mark已经执行拍照
/// 设置RunnableModule的坐标校正器
/// </summary>
/// <param name="module"></param>
public void SetRunnableModuleTransformer(RunnableModule module)
{
if (module == null)
{
return;
}
PointD mark1 = new PointD(), real1 = new PointD(), mark2 = new PointD(), real2 = new PointD();
double markRotation = 0, realRotation = 0;
Dictionary <MarkCmd, PointD> marks = markInfoMap[module];
MarkCmd singleMark = null;
if (marks.Count <= 0)
{
return;
}
int count = 0;
foreach (MarkCmd markCmd in marks.Keys)
{
if (count == 0)
{
singleMark = markCmd;
//非标ASVMark
if (singleMark.ModelFindPrm.IsUnStandard)
{
var structure = module.CommandsModule.program.ModuleStructure;
PointD p = new PointD(singleMark.ModelFindPrm.ReferenceX, singleMark.ModelFindPrm.ReferenceY);
PointD p2 = new PointD(singleMark.ModelFindPrm.ReferenceX2, singleMark.ModelFindPrm.ReferenceY2);
mark1.CopyFrom(structure.ToMachine(module, p));
real1.CopyFrom(singleMark.ModelFindPrm.TargetInMachine);
if (singleMark.ModelFindPrm.UnStandardType == 0)
{
markRotation = singleMark.ModelFindPrm.ReferenceA;
realRotation = singleMark.ModelFindPrm.Angle;
}
else
{
mark2.CopyFrom(structure.ToMachine(module, p2));
real2.CopyFrom(singleMark.ModelFindPrm.TargetInMachine2);
}
}
//正常Mark
else
{
mark1.CopyFrom(markCmd.Position);//编程时
real1.CopyFrom(marks[markCmd]);
}
}
else if (count == 1)
{
mark2.CopyFrom(markCmd.Position);
real2.CopyFrom(marks[markCmd]);
// 脚本语法上限制了最多添加两个Mark点，此处可直接跳出循环
break;
}
count++;
}
CoordinateTransformer transformer = new CoordinateTransformer();
if (marks.Count == 1)
{
Log.Dprint(TAG, "runnable mark count is 1, standard pos : " + mark1 + ", real pos : " + real1);
if (singleMark.ModelFindPrm.IsUnStandard)
{
if (singleMark.ModelFindPrm.UnStandardType == 0)
{
transformer.SetMarkPoint(mark1, markRotation, real1, realRotation);
}
else
{
transformer.SetMarkPoint(mark1, mark2, real1, real2);
}
}
else
{
transformer.SetMarkPoint(mark1, real1);
}
}
else
{
Log.Dprint(TAG, "runnable mark count is 2, standard pos1 : " + mark1 + ", real pos1 : " + real1
+ ", standard pos2 : " + mark2 + ", real pos2 : " + real2);
transformer.SetMarkPoint(mark1, mark2, real1, real2);
}
transMap.Add(module, transformer);
}
