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