/// <summary>非同步移動到某個位置</summary> /// <param name="targetPosition">目標位置</param> /// <remarks>King, 2020/05/25 Add</remarks> private void MoveAsync(HalRobotMotion targetPosition) { this.Robot.HalMoveStraightAsyn(targetPosition); while (!this.Robot.HalMoveIsComplete()) { Thread.Sleep(100); } this.Robot.HalMoveEnd(); }
/// <summary>顯示 Get Button 所得的Position資料</summary> /// <param name="instance"></param> private void DisplayGetPosition(HalRobotMotion instance) { this.LstBxGetPosition.Items.Clear(); PropertyInfo[] properties = typeof(HalRobotMotion).GetProperties(); var text = ""; foreach (var property in properties) { if (property.CanRead) { var axisName = property.Name; var value = property.GetValue(instance); text += axisName + ": " + value.ToString() + ", "; } } text += "Speed: " + instance.Speed + ", MotionType: " + instance.MotionType.ToString(); this.LstBxGetPosition.Items.Add(text); }
/// <summary>從Home 移動到 Cabinet 某個 Box</summary> /// <param name="cabinetIndex">Drawer index</param> /// <param name="boxIndex">Box index</param> /// <remarks>King, 2020/05/25 Add</remarks> private void ForwardToCabinet(int cabinetIndex, int boxIndex) { Func <HalRobotMotion> GetBoxPosition = () => { HalRobotMotion halRobotMotion = null; if (cabinetIndex == 1) { // 終點 // TODO: 要獲得實際的 Position halRobotMotion = new HalRobotMotion { X = 3.3f, Y = -438.782349f, Z = 140.75882f, W = 90.0f, P = -90.0f, R = 0.0f, E1 = 517.0209f, J1 = -90.0f, J2 = 1.134951f, J3 = -50.0000153f, J4 = 0.0f, J5 = 50.000002f, J6 = 0.0f, J7 = 517.0209f, Speed = 60, MotionType = HalRobotEnumMotionType.Position }; } else { } return(halRobotMotion); }; var positions = GetBoxPosition(); this.MoveAsync(positions); }
/// <summary> /// 使用CSV檔Config Default Position /// </summary> /// <param name="CsvPath"></param> public void ReadPathInfo(string CsvPath) { DataTable pathDt = readCSV(CsvPath); for (int k = 1; k < pathDt.Rows.Count - 1; k++) { HalRobotMotion robotMotion = new HalRobotMotion(); switch (pathDt.Rows[k][10].ToString()) { case "W": robotMotion.MotionType = HalRobotEnumMotionType.Position; robotMotion.X = float.Parse(pathDt.Rows[k][2].ToString()); robotMotion.Y = float.Parse(pathDt.Rows[k][3].ToString()); robotMotion.Z = float.Parse(pathDt.Rows[k][4].ToString()); robotMotion.W = float.Parse(pathDt.Rows[k][5].ToString()); robotMotion.P = float.Parse(pathDt.Rows[k][6].ToString()); robotMotion.R = float.Parse(pathDt.Rows[k][7].ToString()); robotMotion.E1 = float.Parse(pathDt.Rows[k][8].ToString()); robotMotion.Speed = int.Parse(pathDt.Rows[k][9].ToString()); btPathInfo.Add(pathDt.Rows[k][1].ToString(), robotMotion); break; case "J": robotMotion.MotionType = HalRobotEnumMotionType.Joint; robotMotion.J1 = float.Parse(pathDt.Rows[k][2].ToString()); robotMotion.J2 = float.Parse(pathDt.Rows[k][3].ToString()); robotMotion.J3 = float.Parse(pathDt.Rows[k][4].ToString()); robotMotion.J4 = float.Parse(pathDt.Rows[k][5].ToString()); robotMotion.J5 = float.Parse(pathDt.Rows[k][6].ToString()); robotMotion.J6 = float.Parse(pathDt.Rows[k][7].ToString()); robotMotion.J7 = float.Parse(pathDt.Rows[k][8].ToString()); robotMotion.Speed = int.Parse(pathDt.Rows[k][9].ToString()); btPathInfo.Add(pathDt.Rows[k][1].ToString(), robotMotion); break; } } }
private void ChangeDirection(HalRobotMotion targetPosition) { // TODO: 待討論 var positionInst = new BoxTransferPathPositions(); HalRobotMotion openStageHome = positionInst.OpenStageHome; HalRobotMotion cabinet1Home = positionInst.Cabinet1Home; HalRobotMotion cabinet2Home = positionInst.Cabinet2Home; bool Licence = false; string StartPosName = ""; string EndPosName = ""; #region 確認Robot是否在三個可以轉動方向的點位內,並確認目前在哪個方位 var StartPosInfo = (this.Robot as HalRobotFanuc).ldd.GetCurrRobotInfo(); if ( StartPosInfo.j1 <= openStageHome.J1 + 5 && StartPosInfo.j1 >= openStageHome.J1 - 5 && StartPosInfo.j2 <= openStageHome.J2 + 5 && StartPosInfo.j2 >= openStageHome.J2 - 5 && StartPosInfo.j3 <= openStageHome.J3 + 5 && StartPosInfo.j3 >= openStageHome.J3 - 5 && StartPosInfo.j4 <= openStageHome.J4 + 5 && StartPosInfo.j4 >= openStageHome.J4 - 5 && StartPosInfo.j5 <= openStageHome.J5 + 5 && StartPosInfo.j5 >= openStageHome.J5 - 5 && StartPosInfo.j6 <= openStageHome.J6 + 5 && StartPosInfo.j6 >= openStageHome.J6 - 5 && StartPosInfo.j7 <= openStageHome.J7 + 5 && StartPosInfo.j7 >= openStageHome.J7 - 5 ) { Licence = true; StartPosName = "OpenStage Home"; } else if ( StartPosInfo.j1 <= cabinet1Home.J1 + 5 && StartPosInfo.j1 >= cabinet1Home.J1 - 5 && StartPosInfo.j2 <= cabinet1Home.J2 + 5 && StartPosInfo.j2 >= cabinet1Home.J2 - 5 && StartPosInfo.j3 <= cabinet1Home.J3 + 5 && StartPosInfo.j3 >= cabinet1Home.J3 - 5 && StartPosInfo.j4 <= cabinet1Home.J4 + 5 && StartPosInfo.j4 >= cabinet1Home.J4 - 5 && StartPosInfo.j5 <= cabinet1Home.J5 + 5 && StartPosInfo.j5 >= cabinet1Home.J5 - 5 && StartPosInfo.j6 <= cabinet1Home.J6 + 5 && StartPosInfo.j6 >= cabinet1Home.J6 - 5 && StartPosInfo.j7 <= cabinet1Home.J7 + 5 && StartPosInfo.j7 >= cabinet1Home.J7 - 5 ) { Licence = true; StartPosName = "Cabinet1 Home"; } else if ( StartPosInfo.j1 <= cabinet2Home.J1 + 5 && StartPosInfo.j1 >= cabinet2Home.J1 - 5 && StartPosInfo.j2 <= cabinet2Home.J2 + 5 && StartPosInfo.j2 >= cabinet2Home.J2 - 5 && StartPosInfo.j3 <= cabinet2Home.J3 + 5 && StartPosInfo.j3 >= cabinet2Home.J3 - 5 && StartPosInfo.j4 <= cabinet2Home.J4 + 5 && StartPosInfo.j4 >= cabinet2Home.J4 - 5 && StartPosInfo.j5 <= cabinet2Home.J5 + 5 && StartPosInfo.j5 >= cabinet2Home.J5 - 5 && StartPosInfo.j6 <= cabinet2Home.J6 + 5 && StartPosInfo.j6 >= cabinet2Home.J6 - 5 && StartPosInfo.j7 <= cabinet2Home.J7 + 5 && StartPosInfo.j7 >= cabinet2Home.J7 - 5 ) { Licence = true; StartPosName = "Cabinet2 Home"; } #endregion #region 確認終點 if (targetPosition.J1 <= openStageHome.J1 + 5 && targetPosition.J1 >= openStageHome.J1 - 5 && targetPosition.J2 <= openStageHome.J2 + 5 && targetPosition.J2 >= openStageHome.J2 - 5 && targetPosition.J3 <= openStageHome.J3 + 5 && targetPosition.J3 >= openStageHome.J3 - 5 && targetPosition.J4 <= openStageHome.J4 + 5 && targetPosition.J4 >= openStageHome.J4 - 5 && targetPosition.J5 <= openStageHome.J5 + 5 && targetPosition.J5 >= openStageHome.J5 - 5 && targetPosition.J6 <= openStageHome.J6 + 5 && targetPosition.J6 >= openStageHome.J6 - 5 && targetPosition.J7 <= openStageHome.J7 + 5 && targetPosition.J7 >= openStageHome.J7 - 5 ) { EndPosName = "OpenStage Target"; } else if (targetPosition.J1 <= cabinet1Home.J1 + 5 && targetPosition.J1 >= cabinet1Home.J1 - 5 && targetPosition.J2 <= cabinet1Home.J2 + 5 && targetPosition.J2 >= cabinet1Home.J2 - 5 && targetPosition.J3 <= cabinet1Home.J3 + 5 && targetPosition.J3 >= cabinet1Home.J3 - 5 && targetPosition.J4 <= cabinet1Home.J4 + 5 && targetPosition.J4 >= cabinet1Home.J4 - 5 && targetPosition.J5 <= cabinet1Home.J5 + 5 && targetPosition.J5 >= cabinet1Home.J5 - 5 && targetPosition.J6 <= cabinet1Home.J6 + 5 && targetPosition.J6 >= cabinet1Home.J6 - 5 && targetPosition.J7 <= cabinet1Home.J7 + 5 && targetPosition.J7 >= cabinet1Home.J7 - 5 ) { EndPosName = "Cabinet1 Target"; } else if (targetPosition.J1 <= cabinet2Home.J1 + 5 && targetPosition.J1 >= cabinet2Home.J1 - 5 && targetPosition.J2 <= cabinet2Home.J2 + 5 && targetPosition.J2 >= cabinet2Home.J2 - 5 && targetPosition.J3 <= cabinet2Home.J3 + 5 && targetPosition.J3 >= cabinet2Home.J3 - 5 && targetPosition.J4 <= cabinet2Home.J4 + 5 && targetPosition.J4 >= cabinet2Home.J4 - 5 && targetPosition.J5 <= cabinet2Home.J5 + 5 && targetPosition.J5 >= cabinet2Home.J5 - 5 && targetPosition.J6 <= cabinet2Home.J6 + 5 && targetPosition.J6 >= cabinet2Home.J6 - 5 && targetPosition.J7 <= cabinet2Home.J7 + 5 && targetPosition.J7 >= cabinet2Home.J7 - 5 ) { EndPosName = "Cabinet2 Target"; } #endregion if (Licence == true) { if (StartPosName != EndPosName && EndPosName != "") { /** * //如果目前位置不在InspCh且要移動的目的地也不是InspCh,則需要先經過InspCh點位再移動到目的地 * if (StartPosName != "Inspection Chamber" && EndPosName != "Inspection Chamber") * { * RobotMove(PosToInspCh()); * RobotMove(PosToAssembly); * } * else * { * RobotMove(PosToAssembly); * } */ cabinet1Home.MotionType = targetPosition.MotionType = HalRobotEnumMotionType.Joint; if (StartPosName != "Cabinet1 Home" && EndPosName != "Cabinet1 Target") { // RobotMove(PosToInspCh()); //RobotMove(PosToAssembly); this.MoveAsync(cabinet1Home); this.MoveAsync(targetPosition); } else { //RobotMove(PosToAssembly); this.MoveAsync(targetPosition); } } else { throw new Exception("Unknown end position !!"); } } else { throw new Exception("Mask robot can not change direction. Because robot is not in the safe range now"); } }
public void TestRobotPathSerialize() { var robotPath = new HalRobotPath() { Name = "Test", Remark = "Test", }; var robotMotion = new HalRobotMotion() { X = 1.1f, Y = 2.2f, Z = 3.3f, W = 4.4f, P = 5.5f, R = 6.6f, E1 = 7.7f, UserFrame = 1, UserTool = 1, Speed = 60, MotionType = HalRobotEnumMotionType.Position, }; robotPath.Motions.Add(robotMotion); robotMotion = robotMotion.Clone(); robotMotion.X += 1; robotPath.Motions.Add(robotMotion); robotMotion = robotMotion.Clone(); robotMotion.X += 1; robotPath.Motions.Add(robotMotion); MvaUtil.SaveXmlToFile(robotPath, robotPath.Name + ".xml"); var loadPath = MvaUtil.LoadFromXmlFile <HalRobotPath>(robotPath.Name + ".xml"); var loadMotion = loadPath.Motions.LastOrDefault(); Assert.AreEqual(robotMotion.X, loadMotion.X); Assert.AreEqual(robotMotion.Z, loadMotion.Z); Assert.AreEqual(robotMotion.Y, loadMotion.Y); Assert.AreEqual(robotMotion.W, loadMotion.W); Assert.AreEqual(robotMotion.P, loadMotion.P); Assert.AreEqual(robotMotion.R, loadMotion.R); Assert.AreEqual(robotMotion.E1, loadMotion.E1); Assert.AreEqual(robotMotion.J1, loadMotion.J1); Assert.AreEqual(robotMotion.J2, loadMotion.J2); Assert.AreEqual(robotMotion.J3, loadMotion.J3); Assert.AreEqual(robotMotion.J4, loadMotion.J4); Assert.AreEqual(robotMotion.J5, loadMotion.J5); Assert.AreEqual(robotMotion.J6, loadMotion.J6); Assert.AreEqual(robotMotion.J7, loadMotion.J7); Assert.AreEqual(robotMotion.UserFrame, loadMotion.UserFrame); Assert.AreEqual(robotMotion.UserTool, loadMotion.UserTool); Assert.AreEqual(robotMotion.Speed, loadMotion.Speed); }
public void SetPosition(HalRobotMotion value) { position = value; }