public int LineAbs(int nRobotIndex, Coordnates nCoordnate, double dbValue)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.LineCoordnate(nCoordnate, MovementsTypes.Abs, SpeedTypes.NoUse, WaitTypes.Wait, dbValue, 0, 0, 0);
return(nRet);
}
public int LineRel(int nRobotIndex, Coordnates nCoordnate, double dbValue, double dbAcc, double dbDec, double dbSpeed)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.LineCoordnate(nCoordnate, MovementsTypes.Rel, SpeedTypes.Use, WaitTypes.Wait, dbValue, dbAcc, dbDec, dbSpeed);
return(nRet);
}
/// <summary>
/// 單一關節Jog移動停止
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <returns>回傳是否成功,0:成功, 其他數字:失敗</returns>
public int JogCoordnateStop(int nRobotIndex, Coordnates nCoordnate)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.JogCoordnateStop(nCoordnate);
return(nRet);
}
/*
* STDMETHOD(LineSingleRel)(LONG lDirection, DOUBLE dbPos, LONG* lRet);
* STDMETHOD(LineSingleRelWait)(LONG lDirection, DOUBLE dbPos, LONG* lRet);
* STDMETHOD(LineSingleRelSpeed)(LONG lDirection, DOUBLE dbPos, DOUBLE dbSpeed, DOUBLE dbAcc, DOUBLE dbDec, LONG* lRet);
* STDMETHOD(LineSingleRelSpeedWait)(LONG lDirection, DOUBLE dbPos, DOUBLE dbSpeed, DOUBLE dbAcc, DOUBLE dbDec, LONG* lRet);
* STDMETHOD(LineSingleAbs)(LONG lDirection, DOUBLE dbPos, LONG* lRet);
* STDMETHOD(LineSingleAbsWait)(LONG lDirection, DOUBLE dbPos, LONG* lRet);
* STDMETHOD(LineSingleAbsSpeed)(LONG lDirection, DOUBLE dbPos, DOUBLE dbSpeed, DOUBLE dbAcc, DOUBLE dbDec, LONG* lRet);
* STDMETHOD(LineSingleAbsSpeedWait)(LONG lDirection, DOUBLE dbPos, DOUBLE dbSpeed, DOUBLE dbAcc, DOUBLE dbDec, LONG* lRet);
*/
/// <summary>
/// 單一關節Jog移動
/// </summary>
/// <param name="nRobotIndex">手臂代號</param>
/// <param name="nJoint">關節代號</param>
/// <param name="nDir">移動方向</param>
/// <param name="dbSpeedPercentage">速度百分比</param>
/// <returns>回傳是否成功,0:成功, 其他數字:失敗</returns>
public int JogCoordnate(int nRobotIndex, Coordnates nCoordnate, Jog_Directions nDir, Jog_Speed nSpeed)
{
IRobotMotion.IRobotMotion IRobot = m_listRobots[nRobotIndex];
int nRet = IRobot.JogCoordnate(nCoordnate, nDir, nSpeed);
return(nRet);
}
//座標Jog 移動停止
public virtual int JogCoordnateStop(Coordnates nCoordate)
{
int nRet = 0;
lock (thisLock)
{
nRet = StopMotionForPtoP(0);
}
return(nRet);
}
//座標Jog 移動停止
public virtual int JogCoordnateStop(Coordnates nJoint)
{
int nRet = 0;
lock (thisLock)
{
nRet = StopMotionForLine(60);
}
return(nRet);
}
//座標Jog 移動
public virtual int JogCoordnate(Coordnates nCoordnate, Jog_Directions nDir, Jog_Speed nSpeed)
{
int nRet = 0;
double dbValue = 0;
lock (thisLock)
{
/*
* if (nSpeed == Jog_Speed.High)
* nRet = SetAllSpeed(LINE, 300, 2000, 2000);
* else if (nSpeed == Jog_Speed.Mid)
* nRet = SetAllSpeed(LINE, 200, 1000, 1000);
* else if (nSpeed == Jog_Speed.Low)
* nRet = SetAllSpeed(LINE, 100, 500, 500);
*/
if (nSpeed == Jog_Speed.High)
{
nRet = SetAllSpeed(LINE, 20, 20, 20);
}
else if (nSpeed == Jog_Speed.Mid)
{
nRet = SetAllSpeed(LINE, 10, 10, 10);
}
else if (nSpeed == Jog_Speed.Low)
{
nRet = SetAllSpeed(LINE, 5, 5, 5);
}
if (nDir == Jog_Directions.Plus)
{
if (nCoordnate == Coordnates.X)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Y)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Z)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Rx)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Ry)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Rz)
{
dbValue = 50;
}
}
else
{
if (nCoordnate == Coordnates.X)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Y)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Z)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Rx)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Ry)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Rz)
{
dbValue = -50;
}
}
nRet = LineCoordnate(nCoordnate, MovementsTypes.Rel, SpeedTypes.NoUse, WaitTypes.NoWait,
dbValue, 0, 0, 0);
}
return(0);
}
public virtual int LineSingleCoordnate(Coordnates nCoordnate, MovementsTypes nMovementType, SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbValue, double dbAccPercentage, double dbDecPercentage, double dbSpeedPercentage)
{
return(1);
}
//單一直角坐標系方向 移動
public virtual int LineCoordnate(Coordnates nCoordnate, MovementsTypes nMovementType, SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbValue, double dbAcc, double dbDec, double dbSpeed)
{
int nErrorCode = 0;
lock (thisLock)
{
try
{
string strCmdHead = "";
string strCmdPos = "";
string strCmdSpeed = "";
string strFinalCmd = "";
//送給控制器的位置順序為 關節代號與位置
strCmdPos = string.Format(",{0:0.###},{1:0.###}", (int)nCoordnate, dbValue);
if (nMovementType == MovementsTypes.Abs)
{
strCmdHead = "LINE_WORLD_SINGLE_ABS";
}
else if (nMovementType == MovementsTypes.Rel)
{
strCmdHead = "LINE_WORLD_SINGLE_REL";
}
if (nSpeedType == SpeedTypes.Use)
{
strCmdHead = strCmdHead + "_SPEED";
strCmdSpeed = string.Format(",{0:0.###},{1:0.###},{2:0.###}", dbSpeed, dbAcc, dbDec);
}
if (nWaitType == WaitTypes.Wait)
{
strCmdHead = strCmdHead + "_WAIT";
}
strFinalCmd = strCmdHead + strCmdPos + strCmdSpeed;
byte[] byteFinalCmd = Encoding.ASCII.GetBytes(strFinalCmd);
byte[] byteRecv = new byte[9];
int nRet = 0;
EpsonClientSendCmd.SendData(byteFinalCmd);
nRet = EpsonClientSendCmd.Receive(ref byteRecv);
if (nRet == 0)
{
if (BitConverter.ToString(byteRecv) != BitConverter.ToString(m_byteSendOK))
{
return(-100);
}
}
else
{
return(-10); //通訊發生錯誤
}
}
catch (Exception ex)
{
nErrorCode = -10;
System.Console.WriteLine("LineCoordnate 例外 : ErrCode = {0} , {1} !", ex.ToString());
return(nErrorCode);
}
}
//等待手臂停止
if (nWaitType == WaitTypes.Wait)
{
string strChkCmd = string.Format("GET_IN_WAIT_FUNC");
byte[] byteChkCmd = Encoding.ASCII.GetBytes(strChkCmd);
byte[] byteRecv = new byte[3];
int nRet = 0;
while (true)
{
System.Threading.Thread.Sleep(1);
lock (thisLock)
{
EpsonClientSendCmd.SendData(byteChkCmd);
nRet = EpsonClientSendCmd.Receive(ref byteRecv);
if (nRet == 0)
{
if (byteRecv[0] == 0x31) //手臂已經不在等待停止函式中了, 離開迴圈
{
break;
}
}
else
{
return(-10); //通訊發生錯誤
}
}
}
}
return(nErrorCode);
}
//單一直角坐標系方向 移動
public virtual int LineCoordnate(Coordnates nCoordnate, MovementsTypes nMovementType, SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbValue, double dbAcc, double dbDec, double dbSpeed)
{
int nRetResult = 0;
lock (thisLock)
{
bool bRetSend = false;
//先記錄目前各世界座標位置()
double dbX = 0, dbY = 0, dbZ = 0, dbRx = 0, dbRy = 0, dbRz = 0;
dbX = URClient.m_tcpPosInfo.dbTCP_X / 1000.0; //轉為 m 單位
dbY = URClient.m_tcpPosInfo.dbTCP_Y / 1000.0; //轉為 m 單位
dbZ = URClient.m_tcpPosInfo.dbTCP_Z / 1000.0; //轉為 m 單位
dbRx = URClient.m_tcpPosInfo.dbTCP_Rx;
dbRy = URClient.m_tcpPosInfo.dbTCP_Ry;
dbRz = URClient.m_tcpPosInfo.dbTCP_Rz;
/*
* dbX = URClient.m_jointRadianInfo.dbBasePosRadian;
* dbY = URClient.m_jointRadianInfo.dbShoulderPosRadian;
* dbZ = URClient.m_jointRadianInfo.dbElbowPosRadian;
* dbRx = URClient.m_jointRadianInfo.dbWrist1PosRadian;
* dbRy = URClient.m_jointRadianInfo.dbWrist2PosRadian;
* dbRz = URClient.m_jointRadianInfo.dbWrist3PosRadian;
* dbValue = dbValue * 3.1415926 / 180.0; //將輸入的變數轉為徑度
*/
/*
* //1.單位選擇
* switch (nUnit)
* {
* case UnitTypes.Degree:
* dbValue = dbValue * 3.1415926 / 180.0; //將輸入的變數轉為徑度
* // dbJ1 = URClient.m_jointRadianInfo.dbBasePosRadian * 3.1415926 / 180.0;
* // dbJ2 = URClient.m_jointRadianInfo.dbShoulderPosRadian * 3.1415926 / 180.0;
* // dbJ3 = URClient.m_jointRadianInfo.dbElbowPosRadian * 3.1415926 / 180.0;
* // dbJ4 = URClient.m_jointRadianInfo.dbWrist1PosRadian * 3.1415926 / 180.0;
* // dbJ5 = URClient.m_jointRadianInfo.dbWrist2PosRadian * 3.1415926 / 180.0;
* // dbJ6 = URClient.m_jointRadianInfo.dbWrist3PosRadian * 3.1415926 / 180.0;
* break;
* case UnitTypes.Radian:
* // dbValue = dbValue ;
* // dbJ1 = URClient.m_jointRadianInfo.dbBasePosRadian;
* // dbJ2 = URClient.m_jointRadianInfo.dbShoulderPosRadian;
* // dbJ3 = URClient.m_jointRadianInfo.dbElbowPosRadian;
* // dbJ4 = URClient.m_jointRadianInfo.dbWrist1PosRadian;
* // dbJ5 = URClient.m_jointRadianInfo.dbWrist2PosRadian;
* // dbJ6 = URClient.m_jointRadianInfo.dbWrist3PosRadian;
*
* break;
* }
*/
/*
* //2.移動量模式選擇
* switch (nMovementType)
* {
* case MovementsTypes.Abs:
* //3.座標選擇
* switch (nCoordnate)
* {
*
* case Coordnates.X: dbX = dbValue / 1000.0; break;
* case Coordnates.Y: dbY = dbValue / 1000.0; break;
* case Coordnates.Z: dbZ = dbValue / 1000.0; break;
* // case Coordnates.Rx: dbRx = dbValue; break;
* //case Coordnates.Ry: dbRy = dbValue; break;
* // case Coordnates.Rz: dbRz = dbValue; break;
* case Coordnates.Rx: dbRx = dbValue * 3.1415926 / 180.0; break;
* case Coordnates.Ry: dbRy = dbValue * 3.1415926 / 180.0; break;
* case Coordnates.Rz: dbRz = dbValue * 3.1415926 / 180.0; break;
*
* }
* break;
* case MovementsTypes.Rel:
* switch (nCoordnate)
* {
*
* case Coordnates.X: dbX = dbX + (dbValue / 1000.0); break;
* case Coordnates.Y: dbY = dbY + (dbValue / 1000.0); break;
* case Coordnates.Z: dbZ = dbZ + (dbValue / 1000.0); break;
* // case Coordnates.Rx: dbRx = dbRx + dbValue; break;
* // case Coordnates.Ry: dbRy = dbRy + dbValue; break;
* // case Coordnates.Rz: dbRz = dbRz + dbValue; break;
* case Coordnates.Rx: dbRx = dbValue; break;
* // case Coordnates.Rx: dbRx = dbRx + (dbValue * 3.1415926 / 180.0); break;
* case Coordnates.Ry: dbRy = dbRy + (dbValue * 3.1415926 / 180.0); break;
* case Coordnates.Rz: dbRz = dbRz + (dbValue * 3.1415926 / 180.0); break;
*
*
* // case Coordnates.X: dbX = dbX + dbValue; break;
* // case Coordnates.Y: dbY = dbY + dbValue; break;
* // case Coordnates.Z: dbZ = dbZ + dbValue; break;
* // case Coordnates.Rx: dbRx = dbRx + dbValue; break;
* // case Coordnates.Ry: dbRy = dbRy + dbValue; break;
* // case Coordnates.Rz: dbRz = dbRz + dbValue; break;
*
* }
* break;
* }
*/
//4.速度型態選擇
//送給UR的速度單位為 rad/s 加速度 rad/s^2
// double dbTempSpeed = (dbSpeed / 100.0) * m_dbMaxLineSpeed;
// double dbTempAcc = (dbAcc / 100.0) * m_dbMaxLineAcc;
double dbTempSpeed = 0.05;
double dbTempAcc = 0.05;
switch (nSpeedType)
{
case SpeedTypes.Use:
string strCmd1 = string.Format("(26,{0},{1},{2},{3},{4},{5},{6},{7},{8},{9})",
dbX, dbY, dbZ, dbRx, dbRy, dbRz, dbTempAcc, dbTempSpeed, 0, 0);
bRetSend = URServer.SendData(strCmd1);
break;
case SpeedTypes.NoUse:
string strCmd2 = string.Format("(21,{0},{1},{2},{3},{4},{5})",
dbX, dbY, dbZ, dbRx, dbRy, dbRz);
bRetSend = URServer.SendData(strCmd2);
break;
}
if (bRetSend == true)
{
//5.等待型態選擇
if (nWaitType == WaitTypes.Wait)
{
byte[] byteRecv;
byteRecv = new byte[32];
while (true)
{
Thread.Sleep(30);
int nLen = URServer.Receive(byteRecv);
if (nLen > 0) // 讀取到資料
{
string result = System.Text.Encoding.UTF8.GetString(byteRecv);
int nRetCompare = string.Compare(result, "MoveL Done");
if (nRetCompare == 0)
{
nRetResult = 0;
}
else
{
nRetResult = -10;
}
break;
}
else //沒有讀取到資料
{
nRetResult = -10;
break;
}
}
}
else
{
nRetResult = 0;
}
}
else
{
nRetResult = -10;
}
}
return(nRetResult);
}
//座標Jog 移動
public virtual int JogCoordnate(Coordnates nCoordnate, Jog_Directions nDir, Jog_Speed nSpeed)
{
double dbValue = 0;
lock (thisLock)
{
double dbSpeed = 0, dbAcc = 0, dbDec = 0;
if (nSpeed == Jog_Speed.High)
{
dbSpeed = 15;
dbAcc = 15;
dbDec = 15;
}
else if (nSpeed == Jog_Speed.Mid)
{
dbSpeed = 8;
dbAcc = 8;
dbDec = 8;
}
else if (nSpeed == Jog_Speed.Low)
{
dbSpeed = 3;
dbAcc = 3;
dbDec = 3;
}
if (nDir == Jog_Directions.Plus)
{
if (nCoordnate == Coordnates.X)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Y)
{
dbValue = 50;
}
else if (nCoordnate == Coordnates.Z)
{
dbValue = 50;
}
//else if (nCoordnate == Coordnates.Rx) dbValue = 20;
else if (nCoordnate == Coordnates.Rx)
{
dbValue = 1.8;
}
else if (nCoordnate == Coordnates.Ry)
{
dbValue = 20;
}
else if (nCoordnate == Coordnates.Rz)
{
dbValue = 20;
}
}
else
{
if (nCoordnate == Coordnates.X)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Y)
{
dbValue = -50;
}
else if (nCoordnate == Coordnates.Z)
{
dbValue = -50;
}
//else if (nCoordnate == Coordnates.Rx) dbValue = -20;
else if (nCoordnate == Coordnates.Rx)
{
dbValue = -1.97;
}
else if (nCoordnate == Coordnates.Ry)
{
dbValue = -20;
}
else if (nCoordnate == Coordnates.Rz)
{
dbValue = -20;
}
}
int nRet = 0;
nRet = LineCoordnate(nCoordnate, MovementsTypes.Rel, SpeedTypes.Use, WaitTypes.NoWait,
dbValue, dbAcc, dbDec, dbSpeed);
}
return(0);
}
