|
|
@@ -912,7 +912,6 @@ namespace TeamAAS_VP.Core
|
|
|
}
|
|
|
#endregion
|
|
|
|
|
|
-
|
|
|
#region PLC
|
|
|
/// <summary>
|
|
|
/// 将系统参数写入PLC
|
|
|
@@ -1066,22 +1065,35 @@ namespace TeamAAS_VP.Core
|
|
|
return;
|
|
|
}
|
|
|
|
|
|
+ // 获取机器人当前坐标
|
|
|
+ var robot = _robotService.GetAllRobots().FirstOrDefault();
|
|
|
+ if (robot == null)
|
|
|
+ {
|
|
|
+ SendTaskMessage($"未找到机器人,无法执行上相机锁附/组装拍照视觉任务", MessageLevel.Alarm);
|
|
|
+ await plc.WriteNodeAsync(addressConfig.Out_UpCameraPutStatus.Address, (Int16)2);
|
|
|
+ return;
|
|
|
+
|
|
|
+ }
|
|
|
+ var currentPosition = robot.GetRobotPos();
|
|
|
+
|
|
|
// 执行视觉任务
|
|
|
var _cts = new CancellationTokenSource();
|
|
|
var visionResult =await _remoteCommandService.ExecutePhotoGetSinglePoint(process, null,null, _cts.Token);
|
|
|
if (visionResult.IsSucceed)
|
|
|
{
|
|
|
- DowmCameraResults[cameraIndex] = new PointF((float)visionResult.X, (float)visionResult.Y);
|
|
|
+ //披头偏差
|
|
|
+ float offsetX = (float)(visionResult.X - currentPosition.X);
|
|
|
+ float offsetY = (float)(visionResult.Y - currentPosition.Y);
|
|
|
+
|
|
|
+ DowmCameraResults[cameraIndex] = new PointF(offsetX, offsetY);
|
|
|
await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
|
|
|
+ return;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)2);
|
|
|
+ return;
|
|
|
}
|
|
|
-
|
|
|
- //_remoteCommandService.ExecuteNormalVisionTaskAsync()
|
|
|
-
|
|
|
- await plc.WriteNodeAsync(addressConfig.Out_DownCameraStatus.Address, (Int16)1);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
@@ -1300,14 +1312,30 @@ namespace TeamAAS_VP.Core
|
|
|
//拍1打1
|
|
|
if (systemConfig.WorkMode == 1)
|
|
|
{
|
|
|
- currentProduct.ScrewPoints[0].X_Position = UpCameraPutResults[1].X;
|
|
|
- currentProduct.ScrewPoints[0].Y_Position = UpCameraPutResults[1].Y;
|
|
|
+ currentProduct.ScrewPoints[0].X_Position = UpCameraPutResults[1].X+ DowmCameraResults[0].X;
|
|
|
+ currentProduct.ScrewPoints[0].Y_Position = UpCameraPutResults[1].Y + DowmCameraResults[0].Y;
|
|
|
await currentProduct.ScrewPoints[0].WriteToPlcAddress(addressConfig.Out_Screw, plc);
|
|
|
}
|
|
|
//拍1打所有
|
|
|
else if (systemConfig.WorkMode == 2)
|
|
|
{
|
|
|
-
|
|
|
+ //根据UpCameraPutResults[]中的索引1、2,和拍照点对应的Local下的坐标,从创建Local坐标系
|
|
|
+ PointF worldP1=new PointF(UpCameraPutResults[1].X, UpCameraPutResults[1].Y);
|
|
|
+ PointF worldP2= new PointF(UpCameraPutResults[2].X, UpCameraPutResults[2].Y);
|
|
|
+ PointF localP1= new PointF(currentProduct.ScrewCameraLocalPos1.X_Position, currentProduct.ScrewCameraLocalPos1.Y_Position);
|
|
|
+ PointF localP2= new PointF(currentProduct.ScrewCameraLocalPos2.X_Position, currentProduct.ScrewCameraLocalPos2.Y_Position);
|
|
|
+ CoordinateTransformer local= new CoordinateTransformer(worldP1, worldP2, localP1, localP2);
|
|
|
+
|
|
|
+ for (int i = 0; i < currentProduct.ScrewPoints.Count; i++)
|
|
|
+ {
|
|
|
+ var localPoint = new PointF(currentProduct.ScrewCameraPoints[i].X_Position, currentProduct.ScrewCameraPoints[i].Y_Position);
|
|
|
+ var worldPoint= local.ToOldCoord(localPoint.X, localPoint.Y);
|
|
|
+ var point = currentProduct.ScrewCameraPoints[i].Clone();
|
|
|
+ point.X_Position = (float)worldPoint[0];
|
|
|
+ point.Y_Position = (float)worldPoint[1];
|
|
|
+ await point.WriteToPlcAddress(addressConfig.Out_ScrewCamera, plc);
|
|
|
+ }
|
|
|
+ await plc.WriteNodeAsync(addressConfig.Out_WorkNum.Address, (Int16)currentProduct.ScrewPoints.Count);
|
|
|
}
|
|
|
await plc.WriteNodeAsync(addressConfig.Out_CameraDone.Address, (Int16)1);
|
|
|
}
|