/// <summary>轉向面對指定的 Cabinet</summary>
/// <param name="cabinetIndex">cabinet index</param>
private void ChangeDirectionToFaceCabinet(int cabinetIndex)
{
var position = default(HalRobotMotion);
if (cabinetIndex == 1)
{
position = new BoxTransferPathPositionsFake().Cabinet1Home;
}
else if (cabinetIndex == 2)
{
position = new BoxTransferPathPositionsFake().Cabinet2Home;
}
ChangeDirection(position);
}
private void ChangeDirection(HalRobotMotion targetPosition)
{ // TODO: 待討論
var positionInst = new BoxTransferPathPositionsFake();
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 GotoStage1()
{
var position = new BoxTransferPathPositionsFake().CabinetHomeToOpenStage01;
this.MoveAsync(position);
}
/// <summary>移至 OpenStage</summary>
/// <remarks>King, 2020/05/25 Add</remarks>
public void ForwardToOpenStage()
{
var position = new BoxTransferPathPositionsFake().OpenStage;
this.MoveAsync(position);
}
/// <summary>轉向面對 Open Statge 的方向 </summary>
public void ChangeDirectionToFaceOpenStage()
{
var position = new BoxTransferPathPositionsFake().OpenStageHome;
ChangeDirection(position);
}
/// <summary>從Open Stage Back Home</summary>
/// <remarks>King, 2020/05/25 Add</remarks>
public void BackwardFromOpenStage()
{
var position = new BoxTransferPathPositionsFake().OpenStageHome;
this.MoveAsync(position);
}
/// <summary>回到 Cabinet 2 Home</summary>
public void BackwardFromCabinet2()
{
var position = new BoxTransferPathPositionsFake().Cabinet2Home;
this.MoveAsync(position);
}