|
|
@@ -62,7 +62,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -108,7 +112,11 @@ namespace TeamAAS_VP.Services
|
|
|
RPoint RobotU = calibrationInfo.CenterPoint.Clone();
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
//移动机器人中心点
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -117,7 +125,11 @@ namespace TeamAAS_VP.Services
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -126,7 +138,11 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -148,7 +164,11 @@ namespace TeamAAS_VP.Services
|
|
|
double offsety = (P0.Y - curPixel.Y) * pixelScaleY;
|
|
|
var pos = RobotCameraRotationMatrix.Inverse() * Vector<double>.Build.Dense(new double[] { offsetx, offsety });
|
|
|
//移动机器人取标定块
|
|
|
- await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -159,7 +179,11 @@ namespace TeamAAS_VP.Services
|
|
|
RobotU.X += (float)pos[0];
|
|
|
RobotU.Y += (float)pos[1];
|
|
|
//机器人移动
|
|
|
- await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -168,7 +192,11 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -186,7 +214,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (Math.Abs(offsetx) < 1 && Math.Abs(offsetx) < 1)
|
|
|
{
|
|
|
//移动机器人取标定块
|
|
|
- await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -194,7 +226,11 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
IsFinish = true;
|
|
|
break;
|
|
|
}
|
|
|
@@ -246,7 +282,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
//计算相机与机器人坐标系的旋转矩阵
|
|
|
var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
|
|
|
@@ -271,7 +311,11 @@ namespace TeamAAS_VP.Services
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
@@ -330,7 +374,11 @@ namespace TeamAAS_VP.Services
|
|
|
Matrix<double> RobotCameraRotationMatrix = null;
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
if (calibrationInfo.Pick.PickPlaceModel == Enums.PickPlaceModel.Inhal)
|
|
|
{
|
|
|
@@ -353,14 +401,14 @@ namespace TeamAAS_VP.Services
|
|
|
}
|
|
|
|
|
|
//计算相机与机器人坐标系的旋转矩阵
|
|
|
- var robotCameraRotationMatrix= await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
|
|
|
+ var robotCameraRotationMatrix = await AutoIdentifyRobotCameraRotationMatrix(calibrationInfo, robot, cancellationToken);
|
|
|
if (!robotCameraRotationMatrix.IsSuccess)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
}
|
|
|
- pixelScaleX= robotCameraRotationMatrix.PixelScaleX;
|
|
|
- pixelScaleY= robotCameraRotationMatrix.PixelScaleY;
|
|
|
- RobotCameraRotationMatrix= robotCameraRotationMatrix.Item2;
|
|
|
+ pixelScaleX = robotCameraRotationMatrix.PixelScaleX;
|
|
|
+ pixelScaleY = robotCameraRotationMatrix.PixelScaleY;
|
|
|
+ RobotCameraRotationMatrix = robotCameraRotationMatrix.Item2;
|
|
|
|
|
|
//自动移动机器人至图像中心点位置
|
|
|
var autoMoveResult = await AutoMoveRobotToImageCenter(robot, RobotCameraRotationMatrix, pixelScaleX, pixelScaleY, cancellationToken);
|
|
|
@@ -368,16 +416,20 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
}
|
|
|
- calibrationInfo.CenterPoint.X= (float)autoMoveResult.X;
|
|
|
- calibrationInfo.CenterPoint.Y= (float)autoMoveResult.Y;
|
|
|
+ calibrationInfo.CenterPoint.X = (float)autoMoveResult.X;
|
|
|
+ calibrationInfo.CenterPoint.Y = (float)autoMoveResult.Y;
|
|
|
|
|
|
|
|
|
RPoint RobotU = calibrationInfo.CenterPoint.Clone();
|
|
|
RobotU.U += (float)calibrationInfo.Angle;
|
|
|
|
|
|
//机器人U轴旋转
|
|
|
- await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
- if(cancellationToken.IsCancellationRequested)
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotU, RobotU.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0, 0, 0);
|
|
|
+ }
|
|
|
+ if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
}
|
|
|
@@ -442,7 +494,7 @@ namespace TeamAAS_VP.Services
|
|
|
(bool IsSuccess, Matrix<double> RotationMatrix, double ToolX, double ToolY, double PixelScaleX, double PixelScaleY) calibresult;
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
- calibresult = await CalibrateFixedDownCameraTool(calibrationInfo,robot,cancellationToken);
|
|
|
+ calibresult = await CalibrateFixedDownCameraTool(calibrationInfo, robot, cancellationToken);
|
|
|
}
|
|
|
else if (calibrationInfo.CameraMount == Enums.CameraMount.FixedUp)
|
|
|
{
|
|
|
@@ -460,10 +512,10 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
return (false, null, 0, 0, 0, 0);
|
|
|
}
|
|
|
-
|
|
|
- pixelScaleX= calibresult.PixelScaleX;
|
|
|
- pixelScaleY= calibresult.PixelScaleY;
|
|
|
- RobotCameraRotationMatrix= calibresult.RotationMatrix;
|
|
|
+
|
|
|
+ pixelScaleX = calibresult.PixelScaleX;
|
|
|
+ pixelScaleY = calibresult.PixelScaleY;
|
|
|
+ RobotCameraRotationMatrix = calibresult.RotationMatrix;
|
|
|
|
|
|
return (true, RobotCameraRotationMatrix, calibresult.ToolX, calibresult.ToolY, pixelScaleX, pixelScaleY);
|
|
|
}
|
|
|
@@ -489,16 +541,24 @@ namespace TeamAAS_VP.Services
|
|
|
|
|
|
//-------------------------------------------P0-----------------------------------
|
|
|
//移动机器人至中心点
|
|
|
-
|
|
|
+ bool isSuccess = false;
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.CenterPoint, calibrationInfo.CenterPoint.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
|
|
|
@@ -511,7 +571,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//相机拍照并处理图像
|
|
|
@@ -526,15 +590,23 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至P0
|
|
|
- await robot.CalibMotionAsync(RobotP0, 0);
|
|
|
- if(cancellationToken.IsCancellationRequested)
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP0, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
+ if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
}
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至P1
|
|
|
- await robot.CalibMotionAsync(RobotP1, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP1, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -542,12 +614,20 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P1
|
|
|
- await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP1, RobotP1.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
@@ -567,7 +647,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至P1
|
|
|
- await robot.CalibMotionAsync(RobotP1, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP1, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -575,7 +659,11 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至P2
|
|
|
- await robot.CalibMotionAsync(RobotP2, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP2, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -583,12 +671,20 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P2
|
|
|
- await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP2, RobotP2.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
@@ -608,7 +704,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至P2
|
|
|
- await robot.CalibMotionAsync(RobotP2, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP2, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -616,7 +716,11 @@ namespace TeamAAS_VP.Services
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
//移动机器人至P0
|
|
|
- await robot.CalibMotionAsync(RobotP0, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP0, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, 0, 0);
|
|
|
@@ -624,12 +728,20 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至P0
|
|
|
- await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(RobotP0, RobotP0.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, 0, 0);
|
|
|
+ }
|
|
|
}
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
@@ -713,7 +825,11 @@ namespace TeamAAS_VP.Services
|
|
|
curpos.X += (float)pos[0];
|
|
|
curpos.Y += (float)pos[1];
|
|
|
//机器人移动
|
|
|
- await robot.CalibMotionAsync(curpos, curpos.Z);
|
|
|
+ bool isSuccess = await robot.CalibMotionAsync(curpos, curpos.Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, 0, 0, 0);
|
|
|
+ }
|
|
|
|
|
|
result = await CaptureAndProcessCallback();
|
|
|
if (!result.IsSuccess)
|
|
|
@@ -780,12 +896,17 @@ namespace TeamAAS_VP.Services
|
|
|
//发送校准的参数
|
|
|
await robot.CalibParameAsync(calibrationInfo.Pick, calibrationInfo.Speed, calibrationInfo.Accel, calibrationInfo.Power, calibrationInfo.WaitSuction, calibrationInfo.WaitBlow);
|
|
|
|
|
|
+ bool isSuccess = false;
|
|
|
var P0 = Vector<double>.Build.Dense(new double[] { calibrationInfo.CenterPoint.X, calibrationInfo.CenterPoint.Y });
|
|
|
if (robot.Brand == Enums.RobotBrand.XYZ_Platform)
|
|
|
{
|
|
|
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ isSuccess=await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -808,7 +929,7 @@ namespace TeamAAS_VP.Services
|
|
|
{
|
|
|
calibrationInfo.MarkPoint = calibrationInfo.CenterPoint.Clone();
|
|
|
}
|
|
|
-
|
|
|
+
|
|
|
calibrationInfo.RobotCameraRotationMatrix = robotCameraRotationMatrix.RotationMatrix.ToArray();
|
|
|
}
|
|
|
|
|
|
@@ -967,7 +1088,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.MobileJ4)
|
|
|
{
|
|
|
//移动机器人至中心位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.CenterPoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -993,7 +1118,11 @@ namespace TeamAAS_VP.Services
|
|
|
//移动机器人至i
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1001,11 +1130,19 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
@@ -1024,7 +1161,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null, null, null);
|
|
|
@@ -1047,7 +1188,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null, null, null);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
@@ -1285,14 +1430,18 @@ namespace TeamAAS_VP.Services
|
|
|
Local = calibrationInfo.CenterPoint.Local,
|
|
|
Tool = calibrationInfo.CenterPoint.Tool
|
|
|
});
|
|
|
-
|
|
|
+ bool isSuccess = false;
|
|
|
List<Vector<double>> TestPos = new List<Vector<double>>();
|
|
|
for (int i = 0; i < rPoints.Count; i++)
|
|
|
{
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
return (false, null);
|
|
|
@@ -1300,12 +1449,20 @@ namespace TeamAAS_VP.Services
|
|
|
//关闭吸气
|
|
|
await robot.CalibOutIOAsync(false);
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(rPoints[i], rPoints[i].Z);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
}
|
|
|
if (cancellationToken.IsCancellationRequested)
|
|
|
{
|
|
|
@@ -1326,7 +1483,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至i
|
|
|
- await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ isSuccess = await robot.CalibMotionAsync(rPoints[i], 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
//打开吸气
|
|
|
await robot.CalibOutIOAsync(true);
|
|
|
}
|
|
|
@@ -1340,7 +1501,11 @@ namespace TeamAAS_VP.Services
|
|
|
if (calibrationInfo.CameraMount == Enums.CameraMount.FixedDown)
|
|
|
{
|
|
|
//移动机器人至待机位置
|
|
|
- await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ isSuccess= await robot.CalibMotionAsync(calibrationInfo.HomePoint, 0);
|
|
|
+ if (!isSuccess)
|
|
|
+ {
|
|
|
+ return (false, null);
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
//RMS(均方根值) 标定偏差
|