public double FindPolarAngle(PointCoord Point) //Finding the Angle of unload tray coords(without camera)
{
double dblX, dblY, dTheta;
try
{
dTheta = 0;
dblX = Point.X;
dblY = Point.Y;
//in tray (unlike carrier, we calculate only the positive quter)
if ((dblX > 0) && (dblY >= 0))
{
dTheta = Math.Atan(dblY / dblX);
}
else if ((dblX > 0) && (dblY < 0))
{
dTheta = Math.Atan(dblY / dblX) + 2 * Math.PI;
}
else if (dblX < 0)
{
dTheta = Math.Atan(dblY / dblX) + Math.PI;
}
else if ((dblX == 0) && (dblY > 0))
{
dTheta = Math.PI / 2;
}
else if ((dblX == 0) && (dblY < 0))
{
dTheta = 3 * Math.PI / 2;
}
return(dTheta);
}
catch { return(0); }
}
public void RotateCoordByAlfa(ref double Pt_x, ref double Pt_y, double Alfa) //for unload tray coords(without camera)
{
//this function take ONE point and rotate it by requested alfa
double dr;
double dTheta; //specifiec index angle (switching from Cartezian coord sys to Polar coord sys)
//alfa, is the all tray angle
try
{
dr = Math.Sqrt(Math.Pow(Pt_x, 2) + Math.Pow(Pt_y, 2)); // dr=(Pt_x^2+Pt_y^2)^0.5
PointCoord tmpPoint = new PointCoord();
tmpPoint.X = Pt_x;
tmpPoint.Y = Pt_y;
dTheta = FindPolarAngle(tmpPoint);
//back to Cartezian coord sys (with the added angle(alfa))
Pt_x = dr * Math.Cos(Alfa + dTheta);
Pt_y = dr * Math.Sin(Alfa + dTheta);
}
catch { }
}
public void LoadingOrder(string OrderName, bool FirstLoad)
{
AppGen.Inst.OrderParams.DeSerialize(OrderName); //from .xml file to OrderParam class
if (AppGen.Inst.OrderParams.InsertCode != OrderName)
{
// return error: order not loaded
}
AppGen.Inst.MDImain.frmOrderEditor.LoadOrderData();
AppGen.Inst.MDImain.frmTitle.LoadOrderData();
if (AppGen.Inst.MDImain.chkVisionON.Checked)
{
AppGen.Inst.MDImain.frmVisionMain.FrmLoadCarrier.InitVision();
AppGen.Inst.MDImain.frmVisionMain.FrmLoadtray.InitVision();
AppGen.Inst.MDImain.frmVisionMain.FrmUnloadCarrier.InitVision();
AppGen.Inst.MDImain.frmVisionMain.FrmLoadCarrier.LoadOrderData();
AppGen.Inst.MDImain.frmVisionMain.FrmLoadtray.LoadOrderData();
AppGen.Inst.MDImain.frmVisionMain.FrmUnloadCarrier.LoadOrderData();
}
AppGen.Inst.AppSettings.CurrentInsertCode = AppGen.Inst.OrderParams.InsertCode;
AppGen.Inst.AppSettings.Serialize();
//creating new instance with the apropriate carriers/tarys according to Order
if (!FirstLoad)
{
AppGen.Inst.LoadCarrier.ResetData();
AppGen.Inst.LoadTray.ResetData();
AppGen.Inst.UnLoadCarrier.ResetData();
AppGen.Inst.UnLoadTray.ResetData();
AppGen.Inst.MainCycle.CounterPerOrder = 0;
AppGen.Inst.MDImain.frmTitle.UpdateFrmTitle(FrmTitleData.CounterPerOrder, AppGen.Inst.MainCycle.CounterPerOrder.ToString());
}
AppGen.Inst.LoadCarrier.ReadFromFile();
AppGen.Inst.LoadTray.ReadFromFile("LoadTray");
AppGen.Inst.UnLoadCarrier.ReadFromFile("UnloadCarrier");
AppGen.Inst.MDImain.frmTitle.progBarLoadTray.Maximum = AppGen.Inst.LoadTray.IndexList.Count;
//----------Unload Tray----------------------------------------------------------------------------------------------------------------------
//reading unload tray file, calculate tray angle, adding calculated angle to tray array and origin, add Rotated origen to Tray array
AppGen.Inst.UnLoadTray.ReadFromFile("UnloadTray");
AppGen.Inst.MDImain.frmTitle.progBarUnloadTray.Maximum = AppGen.Inst.UnLoadTray.IndexList.Count;
PointCoord VectorDelta = new PointCoord(); //for calculate tray direction (by useing Calib 2 points)
VectorDelta.X = AppGen.Inst.VisionParam.UnloadTrayCalibPt[0].X - AppGen.Inst.VisionParam.UnloadTrayCalibPt[3].X;
VectorDelta.Y = AppGen.Inst.VisionParam.UnloadTrayCalibPt[0].Y - AppGen.Inst.VisionParam.UnloadTrayCalibPt[3].Y;
AppGen.Inst.UnLoadTray.TrayAlfa = Math.Atan(VectorDelta.Y / VectorDelta.X);
AppGen.Inst.UnLoadTray.TrayAlfa = Math.Abs(AppGen.Inst.UnLoadTray.TrayAlfa) * -1; //always minus (atan can return plus in some cases)
//AppGen.Inst.UnLoadTray.TrayAlfa = AppGen.Inst.Calculate.FindPolarAngle(VectorDelta);
//AppGen.Inst.UnLoadTray.TrayAlfa = -(Math.PI / 2); // -90 deg from robot coord system //for testing
for (int ii = 0; ii < AppGen.Inst.UnLoadTray.IndexList.Count; ii++)
{
double X = AppGen.Inst.UnLoadTray.IndexList[ii].X_file;
double Y = AppGen.Inst.UnLoadTray.IndexList[ii].Y_file;
AppGen.Inst.Calculate.RotateCoordByAlfa(ref X, ref Y, AppGen.Inst.UnLoadTray.TrayAlfa);
AppGen.Inst.UnLoadTray.IndexList[ii].X_VisRes = X;
AppGen.Inst.UnLoadTray.IndexList[ii].Y_VisRes = Y;
AppGen.Inst.UnLoadTray.IndexList[ii].Angle_VisRes = AppGen.Inst.UnLoadTray.TrayAlfa * (180 / Math.PI); //converted to degree;
}
//double A = AppGen.Inst.VisionParam.xyUnloadWorldTrayOrigin.X;
//double B = AppGen.Inst.VisionParam.xyUnloadWorldTrayOrigin.Y;
//AppGen.Inst.Calculate.RotateCoordByAlfa(ref A, ref B, AppGen.Inst.UnLoadTray.TrayAlfa);
//AppGen.Inst.VisionParam.xyUnloadWorldTrayOriginRotated.X = A;
//AppGen.Inst.VisionParam.xyUnloadWorldTrayOriginRotated.Y = B;
for (int ii = 0; ii < AppGen.Inst.UnLoadTray.IndexList.Count; ii++)
{
AppGen.Inst.UnLoadTray.IndexList[ii].X_VisRes += AppGen.Inst.VisionParam.xyUnloadWorldTrayOrigin.X;
AppGen.Inst.UnLoadTray.IndexList[ii].Y_VisRes += AppGen.Inst.VisionParam.xyUnloadWorldTrayOrigin.Y;
}
//----------------------------------------------------------------------------------------------------------------------------------------
AppGen.Inst.MDImain.frmTitle.UpdateFrmTitle(FrmTitleData.AreaIndexLoadTray, AppGen.Inst.LoadTray.CurrIndex.ToString());
AppGen.Inst.MDImain.frmTitle.UpdateFrmTitle(FrmTitleData.AreaIndexLoadCarrier, AppGen.Inst.LoadCarrier.CurrIndex.ToString());
AppGen.Inst.MDImain.frmTitle.UpdateFrmTitle(FrmTitleData.AreaIndexUnloadTray, AppGen.Inst.UnLoadTray.CurrIndex.ToString());
AppGen.Inst.MainCycle.UnloadCarrierSliceNo = 1;
AppGen.Inst.MDImain.frmTitle.UpdateFrmTitle(FrmTitleData.AreaSliceUnloadCarrier, AppGen.Inst.MainCycle.UnloadCarrierSliceNo.ToString());
}