/// <summary>
/// 计算单工位角度
/// </summary>
/// <param name="index">工位号</param>
/// <param name="rc">旋转中心</param>
/// <param name="cameraNo">相机编号</param>
/// <param name="disMark">mark间距</param>
/// <param name="srcAngle">起始角度</param>
/// <param name="dstAngle">终点角度</param>
/// <param name="botAngle">相机朝上，铭牌和机器人Y轴的夹角（逆时针）</param>
/// <returns></returns>
public Point3D Calc(int index, Point2D rc, int cameraNo, double disMark, double srcAngle, double dstAngle, double botAngle)
{
Point3D pt3Result = new Point3D();
//数组索引从0开始
--index;
try
{
//用于计算结果的工具类
FunCalibRotate funCalibRotate = new FunCalibRotate();
//计算当前位置与标定位置的角度差
double deltar = CalibDataMngr.instance.CurAngle(disMark, cameraNo) - CalibDataMngr.instance.CalibPos_L[index].DblValue4;
//计算旋转之后的mark位置
Point2D dblMark1AfterR = funCalibRotate.GetPoint_AfterRotation(deltar / 180 * Math.PI, rc, CalibDataMngr.instance.pt2Mark1);
//计算xy和标定位置的偏差
double dblDeltaX = dblMark1AfterR.DblValue1 - CalibDataMngr.instance.CalibPos_L[index].DblValue1;
double dblDeltaY = dblMark1AfterR.DblValue2 - CalibDataMngr.instance.CalibPos_L[index].DblValue2;
//根据当前角度和目标位置角度计算实际偏差
Point2D delta = TransDelta(new Point2D(dblDeltaX * AMP, dblDeltaY * AMP), srcAngle, dstAngle);
//给计算结果赋值
pt3Result.DblValue1 = delta.DblValue1;
pt3Result.DblValue2 = delta.DblValue2;
pt3Result.DblValue3 = deltar;
}
catch (Exception ex)
{
Log.L_I.WriteError(NameClass, ex);
}
return(pt3Result);
}
/// <summary>
/// 将视觉偏差转换为最终偏差
/// </summary>
/// <returns></returns>
public Point2D GetRealDelta()
{
try
{
FunCalibRotate fun = new FunCalibRotate();
//角度分两块 1 精确定位到残边平台
//2 残边平台在转移到机器人坐标系
//相当于将一组偏差转两次，设置好对应关系后相加即可
//先得到实际坐标系偏差
DeltaReal = fun.GetPoint_AfterRotation(
AnglePreciToITOPlat / 180 * Math.PI, new Point2D(0, 0), DeltaVision);
DeltaRobot = fun.GetPoint_AfterRotation(
((double)ParSTDArrange.P_I.TypeRobotCoor_E) / 180 * Math.PI, new Point2D(0, 0), DeltaReal);
//认为机器人的偏差位最终偏差
return(DeltaReal);
}
catch (Exception ex)
{
Log.L_I.WriteError("MainSTDArrangeProd.GetRealDelta", ex);
return(new Point2D());
}
}
public void DualLocation(int index)
{
if (Camera1Done & Camera2Done)
{
ShowState("开始计算工位" + index + "偏差");
BaseParCalibrate baseParComprehensive = ParComprehensive2.P_I.GetCellParCalibrate(Camera2RC);
ParCalibRotate parCalibRotate = (ParCalibRotate)baseParComprehensive;
double angle = Math.Atan(
(Pt2Mark2.DblValue2
- Pt2Mark1.DblValue2)
* AMP
/ Protocols.confDisMark) * 180 / Math.PI
- StationDataMngr.CalibPos_L[index - 1].DblValue4;
ShowState("工位" + index + "逆时针角度偏差: " + angle);
FunCalibRotate fcr = new FunCalibRotate();
Point2D MarkAfterRotate = fcr.GetPoint_AfterRotation(
angle / 180 * Math.PI, parCalibRotate.PointRC, Pt2Mark2);
double deltaY = MarkAfterRotate.DblValue1 - StationDataMngr.CalibPos_L[index - 1].DblValue1;
double deltaX = MarkAfterRotate.DblValue2 - StationDataMngr.CalibPos_L[index - 1].DblValue2;
deltaX *= AMP;
deltaY *= -AMP;
Point2D delta = TransDelta(new Point2D(deltaX, deltaY),
Protocols.ConfPlaceAngle, Protocols.ConfPreciseAngle);
ShowState(string.Format("工位{0}X方向补偿{1},Y方向补偿{2}", index, delta.DblValue1.ToString(ReserveDigits), delta.DblValue2.ToString(ReserveDigits)));
int num = (StationNum - 1) * 2 + index;
Point4D pos = new Point4D
{
DblValue1 = delta.DblValue1
+ StationDataMngr.PlacePos_L[index - 1].DblValue1
+ ParAdjust.Value1("adj" + num),
DblValue2 = delta.DblValue2
+ StationDataMngr.PlacePos_L[index - 1].DblValue2
+ ParAdjust.Value2("adj" + num),
DblValue3 = StationDataMngr.PlacePos_L[index - 1].DblValue3,
DblValue4 = Protocols.ConfPlaceAngle + angle + ParAdjust.Value3("adj" + num)
};
LogicRobot.L_I.WriteRobotCMD(pos, Protocols.BotCmd_StationPos);
ShowState("发送机器人放片坐标：" + pos.DblValue1.ToString("f3") + @"/" + pos.DblValue2.ToString("f3"));
}
}
