/// <summary>
/// 获取当前点的坐标系坐标
/// </summary>
/// <param name="type"></param>
/// 0 - NO COMPENSATION
/// 1 - AFFINITY COMPENSATION
/// 2 - LINE COMPENSATION
/// <returns></returns>
public static Vector Get_Coordinate(int type)
{
Vector Result = new Vector(0, 0);
double[] Curent_Pos = new double[2];
MC.GT_GetCrdPos(1, out Curent_Pos[0]);
Vector Tem_Pos = new Vector(-(decimal)Curent_Pos[0] / Para_List.Parameter.Gts_Pos_reference, -(decimal)Curent_Pos[1] / Para_List.Parameter.Gts_Pos_reference);
//calculate data
if (type == 0)
{
Result = new Vector(Tem_Pos);
}
else
{
if (Para_List.Parameter.Gts_Affinity_Type == 2)//线性
{
Result = new Vector(Gts_Cal_Data_Resolve.Get_Line_Fit_Coordinate_MA(Tem_Pos.X, Tem_Pos.Y, Fit_Matrices_MA));
}
else
{
Result = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(1, Tem_Pos.X, Tem_Pos.Y, affinity_Matrices));
}
}
return(Result);
}
//XY平台运动到指定点位
public static void Gts_Ready_Correct(Vector Point)
{
//数据矫正
Vector Tmp_Point = new Vector();
//定义处理的变量
decimal Tmp_X = 0.0m;
decimal Tmp_Y = 0.0m;
//终点计算
//数据矫正
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(0, Point.X, Point.Y, affinity_Matrices));
Tmp_X = Tmp_Point.X;
Tmp_Y = Tmp_Point.Y;
//清除FIFO 0
Clear_FIFO();
//初始化FIFO 0前瞻模块
Gts_Return = MC.GT_InitLookAhead(1, 0, Convert.ToDouble(Para_List.Parameter.LookAhead_EvenTime), Convert.ToDouble(Para_List.Parameter.LookAhead_MaxAcc), 4096, ref crdData[0]);
Log.Commandhandler("Line_Interpolation--初始化FIFO 0前瞻模块", Gts_Return);
//直线插补定位
Line_FIFO(Tmp_X, Tmp_Y);//将直线插补数据写入
//将前瞻数据压入控制器
Gts_Return = MC.GT_CrdData(1, Crd_IntPtr, 0);
Log.Commandhandler("Line_Interpolation--将前瞻数据压入控制器", Gts_Return);
//启动定位
Interpolation_Start();
}
/// <summary>
/// 定位到指定点，type：0 - 不矫正，1 - 矫正
/// </summary>
/// <param name="Point"></param>
/// <param name="type"></param>
public bool Gts_Point_Go(Vector Point, int type)
{
//平台安全防护
if (Program.SystemContainer.SysPara.Safe_moveEntrench == 1)//0 - 不启用运动安全防护；其他 - 启用
{
if (Program.SystemContainer.IO.SafeSensorCheck.Equals(1))
{
MessageBox.Show("运动安全防护触发，请移除障碍物！！！");
return(false);
}
}
#if !DEBUG
//数据矫正
Vector Tmp_Point = new Vector();
//定义处理的变量
decimal Tmp_X = 0.0m;
decimal Tmp_Y = 0.0m;
//终点计算
if (type == 0)
{
Tmp_X = Point.X;
Tmp_Y = Point.Y;
}
else if (type == 1)
{
//数据矫正
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(0, Point.X, Point.Y));
Tmp_X = Tmp_Point.X;
Tmp_Y = Tmp_Point.Y;
}
//坐标极限判断
if ((Tmp_X > Program.SystemContainer.SysPara.Work.X) || (Tmp_X < -2) || (Tmp_Y > Program.SystemContainer.SysPara.Work.Y) || (Tmp_Y < -(Program.SystemContainer.SysPara.Rtc_Org.Y + 1)))
{
MessageBox.Show(string.Format("坐标({0},{1})超出平台定位极限！！！", Tmp_X, Tmp_Y));
return(false);
}
//启动定位
Interpolation_Start(Tmp_X, Tmp_Y);
//刷新坐标
RefreshPoint?.Invoke();
#endif
//检测是否有错误发生
if (Program.SystemContainer.IO.Axis01_Err_Occur || Program.SystemContainer.IO.Axis02_Err_Occur)
{
return(false);
}
else
{
return(true);
}
}
/// <summary>
/// 获取当前点的坐标系坐标 0 - NO COMPENSATION;1 - AFFINITY COMPENSATION;
/// </summary>
/// <param name="type"></param>
/// 0 - NO COMPENSATION
/// 1 - AFFINITY COMPENSATION
/// 2 - LINE COMPENSATION
/// <returns></returns>
public Vector Get_Coordinate(int type)
{
Vector Result = new Vector(0, 0);
Vector Tem_Pos = new Vector(0, 0);
/***********人为单轴拆分***************/
double Current_Pos_X, Current_Pos_Y;
MC.GT_GetPrfPos(1, out Current_Pos_X, 1, out uint pXClock); //读取X轴当前位置
MC.GT_GetPrfPos(2, out Current_Pos_Y, 1, out uint pYClock); //读取Y轴当前位置
Tem_Pos = new Vector(Program.SystemContainer.SysPara.Work.X - (decimal)Current_Pos_X / Program.SystemContainer.SysPara.Gts_Pos_reference, Program.SystemContainer.SysPara.Work.Y - (decimal)Current_Pos_Y / Program.SystemContainer.SysPara.Gts_Pos_reference);
/***********输出结果***************/
if (type == 0)
{
Result = new Vector(Tem_Pos);
}
else
{
Result = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(1, Tem_Pos.X, Tem_Pos.Y));
}
return(Result);
}
/// <summary>
/// XY平台运动到配合振镜切割准备点 坐标矫正4
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
public static void Gts_Ready_Correct(decimal x, decimal y)
{
//数据矫正
Vector Tmp_Point = new Vector();
//定义处理的变量
decimal Tmp_X = 0.0m;
decimal Tmp_Y = 0.0m;
if (Para_List.Parameter.Gts_Affinity_Type == 2)
{
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Line_Fit_Coordinate_AM(x, y, Fit_Matrices_AM));
Tmp_X = Tmp_Point.X;
Tmp_Y = Tmp_Point.Y;
}
else
{
//数据矫正
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(0, x, y, affinity_Matrices));
Tmp_X = Tmp_Point.X;
Tmp_Y = Tmp_Point.Y;
}
#if !DEBUG
//清除FIFO 0
Clear_FIFO();
//初始化FIFO 0前瞻模块
Gts_Return = MC.GT_InitLookAhead(1, 0, Convert.ToDouble(Para_List.Parameter.LookAhead_EvenTime), Convert.ToDouble(Para_List.Parameter.LookAhead_MaxAcc), 4096, ref crdData[0]);
Log.Commandhandler("Line_Interpolation--初始化FIFO 0前瞻模块", Gts_Return);
//直线插补定位
Line_FIFO(Tmp_X, Tmp_Y);//将直线插补数据写入
//将前瞻数据压入控制器
Gts_Return = MC.GT_CrdData(1, Crd_IntPtr, 0);
Log.Commandhandler("Line_Interpolation--将前瞻数据压入控制器", Gts_Return);
#endif
//启动定位
Interpolation_Start();
}
/// <summary>
/// List<Interpolation_Data> 有前瞻和坐标矫正的 FIFO数据追加
/// </summary>
/// <param name="Concat_Datas"></param>
public static void Tran_Data_Correct(List <Interpolation_Data> Concat_Datas)
{
#if !DEBUG
//清除FIFO 0
Clear_FIFO();
//初始化FIFO 0前瞻模块
Gts_Return = MC.GT_InitLookAhead(1, 0, Convert.ToDouble(Para_List.Parameter.LookAhead_EvenTime), Convert.ToDouble(Para_List.Parameter.LookAhead_MaxAcc), 4096, ref crdData[0]);
Log.Commandhandler("Line_Interpolation--初始化FIFO 0前瞻模块", Gts_Return);
#endif
//定义处理的变量
Vector Tmp_Point = new Vector();
decimal Tmp_End_X = 0.0m;
decimal Tmp_End_Y = 0.0m;
decimal Tmp_Center_X = 0.0m;
decimal Tmp_Center_Y = 0.0m;
decimal Tmp_Center_Start_X = 0.0m;
decimal Tmp_Center_Start_Y = 0.0m;
foreach (var o in Concat_Datas)
{
if (Para_List.Parameter.Gts_Affinity_Type == 2)
{
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Line_Fit_Coordinate_AM(o.End_x, o.End_y, Fit_Matrices_AM));
Tmp_End_X = Tmp_Point.X;
Tmp_End_Y = Tmp_Point.Y;
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Line_Fit_Coordinate_AM(o.Center_x, o.Center_y, Fit_Matrices_AM));
Tmp_Center_X = Tmp_Point.X;
Tmp_Center_Y = Tmp_Point.Y;
}
else
{
//数据矫正
//终点计算
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(0, o.End_x, o.End_y, affinity_Matrices));
Tmp_End_X = Tmp_Point.X;
Tmp_End_Y = Tmp_Point.Y;
//圆心计算
Tmp_Point = new Vector(Gts_Cal_Data_Resolve.Get_Affinity_Point(0, o.Center_x, o.Center_y, affinity_Matrices));
Tmp_Center_X = Tmp_Point.X;
Tmp_Center_Y = Tmp_Point.Y;
}
//圆心与差值计算
Tmp_Center_Start_X = Tmp_Center_X - Tmp_End_X;
Tmp_Center_Start_Y = Tmp_Center_X - Tmp_End_Y;
#if !DEBUG
//替换数据
if (o.Type == 1) //直线
{
Line_FIFO(Tmp_End_X, Tmp_End_Y); //将直线插补数据写入
}
else if (o.Type == 2) //圆弧
{
Circle_R_FIFO(Tmp_End_X, Tmp_End_Y, o.Circle_radius, o.Circle_dir); //将圆弧插补写入
}
else if (o.Type == 3) //圆形
{
Circle_C_FIFO(Tmp_End_X, Tmp_End_Y, Tmp_Center_Start_X, Tmp_Center_Start_Y, o.Circle_dir); //将圆形插补写入
}
#endif
}
#if !DEBUG
//将前瞻数据压入控制器
Gts_Return = MC.GT_CrdData(1, Crd_IntPtr, 0);
Log.Commandhandler("Line_Interpolation--将前瞻数据压入控制器", Gts_Return);
#endif
}
