/// <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 }