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 MoveJoint(Joints nJoint, MovementsTypes nMovementType, SpeedTypes nSpeedUsed, WaitTypes nWaitType,
double dbValue, double dbAcc, double dbDec, double dbSpeed)
{
int nErrorCode = 0;
lock (thisLock)
{
try
{
double dbJ1 = 0, dbJ2 = 0, dbJ3 = 0, dbJ4 = 0, dbJ5 = 0, dbJ6 = 0;
dbJ1 = m_CurrJointPos.dbJ1;
dbJ2 = m_CurrJointPos.dbJ2;
dbJ3 = m_CurrJointPos.dbJ3;
dbJ4 = m_CurrJointPos.dbJ4;
dbJ5 = m_CurrJointPos.dbJ5;
dbJ6 = m_CurrJointPos.dbJ6;
//關節選擇
switch (nJoint)
{
case Joints.J1: dbJ1 = dbValue; break;
case Joints.J2: dbJ2 = dbValue; break;
case Joints.J3: dbJ3 = dbValue; break;
case Joints.J4: dbJ4 = dbValue; break;
case Joints.J5: dbJ5 = dbValue; break;
case Joints.J6: dbJ6 = dbValue; break;
}
string strCmdHead = "";
string strCmdPos = "";
string strCmdSpeed = "";
string strFinalCmd = "";
strCmdPos = string.Format(",{0:0.###},{1:0.###},{2:0.###},{3:0.###},{4:0.###},{5:0.###}",
dbJ1, dbJ2, dbJ3, dbJ4, dbJ5, dbJ6);
if (nMovementType == MovementsTypes.Abs)
{
strCmdHead = "MOVE_JOINTS_ABS";
}
else if (nMovementType == MovementsTypes.Rel)
{
strCmdHead = "MOVE_JOINTS_REL";
}
if (nSpeedUsed == 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("MoveJoint 例外 : 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 MoveAllJoint(MovementsTypes nMovementType, UnitTypes nUnit, SpeedTypes nSpeedUsed, WaitTypes nWaitType,
double dbJ1Value, double dbJ2Value, double dbJ3Value, double dbJ4Value, double dbJ5Value, double dbJ6Value, double dbAcc, double dbDec, double dbSpeed)
{
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 MoveAllJoint(MovementsTypes nMovementType, UnitTypes nUnit, SpeedTypes nSpeedUsed, WaitTypes nWaitType,
double dbJ1Value, double dbJ2Value, double dbJ3Value, double dbJ4Value, double dbJ5Value, double dbJ6Value, double dbAcc, double dbDec, double dbSpeed)
{
int nRetResult = 0;
lock (thisLock)
{
bool bRetSend = false;
//先記錄目前各關節位置
double dbJ1 = 0, dbJ2 = 0, dbJ3 = 0, dbJ4 = 0, dbJ5 = 0, dbJ6 = 0;
//1.單位選擇
switch (nUnit)
{
case UnitTypes.Degree:
dbJ1 = dbJ1Value * 3.1415926 / 180.0;;
dbJ2 = dbJ2Value * 3.1415926 / 180.0;;
dbJ3 = dbJ3Value * 3.1415926 / 180.0;;
dbJ4 = dbJ4Value * 3.1415926 / 180.0;;
dbJ5 = dbJ5Value * 3.1415926 / 180.0;;
dbJ6 = dbJ6Value * 3.1415926 / 180.0;;
break;
case UnitTypes.Radian:
dbJ1 = dbJ1Value;
dbJ2 = dbJ2Value;
dbJ3 = dbJ3Value;
dbJ4 = dbJ4Value;
dbJ5 = dbJ5Value;
dbJ6 = dbJ6Value;
break;
}
//2.移動量模式選擇
switch (nMovementType)
{
case MovementsTypes.Abs:
break;
case MovementsTypes.Rel:
dbJ1 = URClient.m_jointRadianInfo.dbBasePosRadian + dbJ1;
dbJ2 = URClient.m_jointRadianInfo.dbShoulderPosRadian + dbJ2;
dbJ3 = URClient.m_jointRadianInfo.dbElbowPosRadian + dbJ3;
dbJ4 = URClient.m_jointRadianInfo.dbWrist1PosRadian + dbJ4;
dbJ5 = URClient.m_jointRadianInfo.dbWrist2PosRadian + dbJ5;
dbJ6 = URClient.m_jointRadianInfo.dbWrist3PosRadian + dbJ6;
break;
}
//4.速度型態選擇
switch (nSpeedUsed)
{
case SpeedTypes.Use:
string strCmd1 = string.Format("(25,{0},{1},{2},{3},{4},{5},{6},{7},{8},{9})",
dbJ1, dbJ2, dbJ3, dbJ4, dbJ5, dbJ6, dbAcc, dbSpeed, 0, 0);
bRetSend = URServer.SendData(strCmd1);
break;
case SpeedTypes.NoUse:
string strCmd2 = string.Format("(20,{0},{1},{2},{3},{4},{5})",
dbJ1, dbJ2, dbJ3, dbJ4, dbJ5, dbJ6);
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, "MoveJ Done");
if (nRetCompare == 0)
{
nRetResult = 1;
}
else
{
nRetResult = 0;
}
break;
}
else //沒有讀取到資料
{
nRetResult = 0;
//if (nLen == SOCKET_ERROR)
//{
// int nErr = WSAGetLastError();
// if (nErr != WSAEWOULDBLOCK)
// {
// bRet = FALSE;
// break;
// }
// }
break;
}
}
}
else
{
nRetResult = 1;
}
}
else
{
nRetResult = 0;
}
}
return(nRetResult);
}
//單一直角坐標系方向 移動
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);
}
//單一關節 移動角度
public virtual int MoveJoint(Joints nJoint, MovementsTypes nMovementType, SpeedTypes nSpeedUsed, WaitTypes nWaitType,
double dbValue, double dbAcc, double dbDec, double dbSpeed)
{
int nRetResult = 0;
lock (thisLock)
{
bool bRetSend = false;
//先記錄目前各關節位置(徑度)
double dbJ1 = 0, dbJ2 = 0, dbJ3 = 0, dbJ4 = 0, dbJ5 = 0, dbJ6 = 0;
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;
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 (nJoint)
{
case Joints.J1: dbJ1 = dbValue; break;
case Joints.J2: dbJ2 = dbValue; break;
case Joints.J3: dbJ3 = dbValue; break;
case Joints.J4: dbJ4 = dbValue; break;
case Joints.J5: dbJ5 = dbValue; break;
case Joints.J6: dbJ6 = dbValue; break;
}
break;
case MovementsTypes.Rel:
switch (nJoint)
{
case Joints.J1: dbJ1 = dbJ1 + dbValue; break;
case Joints.J2: dbJ2 = dbJ2 + dbValue; break;
case Joints.J3: dbJ3 = dbJ3 + dbValue; break;
case Joints.J4: dbJ4 = dbJ4 + dbValue; break;
case Joints.J5: dbJ5 = dbJ5 + dbValue; break;
case Joints.J6: dbJ6 = dbJ6 + dbValue; break;
}
break;
}
//4.速度型態選擇
//送給UR的速度單位為 rad/s 加速度 rad/s^2
double dbTempSpeed = (dbSpeed / 100.0) * m_dbMaxJointSpeed;
double dbTempAcc = (dbAcc / 100.0) * m_dbMaxJointAcc;
switch (nSpeedUsed)
{
case SpeedTypes.Use:
string strCmd1 = string.Format("(25,{0},{1},{2},{3},{4},{5},{6},{7},{8},{9})",
dbJ1, dbJ2, dbJ3, dbJ4, dbJ5, dbJ6, dbTempAcc, dbTempSpeed, 0, 0);
bRetSend = URServer.SendData(strCmd1);
break;
case SpeedTypes.NoUse:
string strCmd2 = string.Format("(20,{0},{1},{2},{3},{4},{5})",
dbJ1, dbJ2, dbJ3, dbJ4, dbJ5, dbJ6);
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, "MoveJ Done");
if (nRetCompare == 0)
{
nRetResult = 0;
}
else
{
nRetResult = -10;
}
break;
}
else //沒有讀取到資料
{
nRetResult = -10;
//if (nLen == SOCKET_ERROR)
//{
// int nErr = WSAGetLastError();
// if (nErr != WSAEWOULDBLOCK)
// {
// bRet = FALSE;
// break;
// }
// }
break;
}
}
}
else
{
nRetResult = 0;
}
}
else
{
nRetResult = -10;
}
}
return(nRetResult);
}
