//在直角坐標系上 圓弧運動
public virtual int Arc(SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbMidXValue, double dbMidYValue, double dbMidZValue, double dbMidRxValue, double dbMidRyValue, double dbMidRzValue,
double dbEndXValue, double dbEndYValue, double dbEndZValue, double dbEndRxValue, double dbEndRyValue, double dbEndRzValue,
double dbAcc, double dbDec, double dbSpeed)
{
return(0);
}
public virtual void GoSmooth(int xDir, int zDir, Vector3 end, SpeedTypes speed, Animator animator)
{
if (lockMove == 0)
{
TurnHead(xDir, zDir);
StartCoroutine(SmoothMovement(end, speed, animator));
}
}
//public void GoSmooth(int xDir, int zDir, SpeedTypes speed)
//{
// Vector3 end = transform.position + new Vector3(xDir, 0f, zDir);
// StartCoroutine(SmoothMovement(end, speed));
//}
protected IEnumerator SmoothMovement(Vector3 end, SpeedTypes speed)
{
StartMove();
float journeyLength = (float)Math.Round((transform.position - end).magnitude, 3);
while (journeyLength > 0f)
{
Vector3 newPostion = Vector3.MoveTowards(rigidBody.position, end, (int)speed * Time.deltaTime);
rigidBody.MovePosition(newPostion);
journeyLength = (float)Math.Round((transform.position - end).magnitude, 3);
yield return(null);
}
StopMove();
}
public virtual bool Move(int xDir, int zDir, out RaycastHit hit, SpeedTypes speed)
{
Vector3 start = transform.position;
Vector3 end = start + new Vector3(xDir, 0f, zDir);
Physics.Linecast(start, end, out hit);
if (hit.transform == null)
{
if (lockMove == 0)
{
TurnHead(xDir, zDir);
StartCoroutine(SmoothMovement(end, speed));
}
return(true);
}
return(false);
}
//單一關節 移動角度
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 Arc(SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbMidXValue, double dbMidYValue, double dbMidZValue, double dbMidRxValue, double dbMidRyValue, double dbMidRzValue,
double dbEndXValue, double dbEndYValue, double dbEndZValue, double dbEndRxValue, double dbEndRyValue, double dbEndRzValue,
double dbAcc, double dbDec, double dbSpeed)
{
int nErrorCode = 0;
lock (thisLock)
{
string strCmdHead = "";
string strCmdMiddlePos = "";
string strCmdEndPos = "";
string strCmdSpeed = "";
string strFinalCmd = "";
//送給控制器的位置順序為 X,Y,Z,Rz,Ry,Rx
strCmdMiddlePos = string.Format(",{0:0.###},{1:0.###},{2:0.###},{3:0.###},{4:0.###},{5:0.###}",
dbMidXValue, dbMidYValue, dbMidZValue, dbMidRzValue, dbMidRyValue, dbMidRxValue);
//送給控制器的位置順序為 X,Y,Z,Rz,Ry,Rx
strCmdEndPos = string.Format(",{0:0.###},{1:0.###},{2:0.###},{3:0.###},{4:0.###},{5:0.###}",
dbEndXValue, dbEndYValue, dbEndZValue, dbEndRzValue, dbEndRyValue, dbEndRxValue);
strCmdHead = "ARC_XYZ_ABS";
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 + strCmdMiddlePos + strCmdEndPos + 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); //通訊發生錯誤
}
}
//等待手臂停止
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 LinePtoP(SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbXValue, double dbYValue, double dbZValue, double dbRxValue, double dbRyValue, double dbRzValue, double dbAcc, double dbDec, double dbSpeed)
{
int nErrorCode = 0;
lock (thisLock)
{
try
{
double dbX = 0, dbY = 0, dbZ = 0, dbRx = 0, dbRy = 0, dbRz = 0;
dbX = dbXValue;
dbY = dbYValue;
dbZ = dbZValue;
dbRx = dbRxValue;
dbRy = dbRyValue;
dbRz = dbRzValue;
string strCmdHead = "";
string strCmdPos = "";
string strCmdSpeed = "";
string strFinalCmd = "";
//送給控制器的位置順序為 X,Y,Z,Rz,Ry,Rx
strCmdPos = string.Format(",{0:0.###},{1:0.###},{2:0.###},{3:0.###},{4:0.###},{5:0.###}",
dbX, dbY, dbZ, dbRz, dbRy, dbRx);
strCmdHead = "LINE_WORLD_ALL_ABS";
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("LinePtoP 例外 : 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 LineSingleCoordnate(Coordnates nCoordnate, MovementsTypes nMovementType, SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbValue, double dbAccPercentage, double dbDecPercentage, double dbSpeedPercentage)
{
return(1);
}
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 Enemy(int intelligence, int numberOfPoints, SpeedTypes speed) : base()
{
this.intelligence = intelligence;
this.numberOfPoints = numberOfPoints;
this.speed = speed;
}
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);
}
//在直角坐標系上 點對點 直線運動
public virtual int LinePtoP(SpeedTypes nSpeedType, WaitTypes nWaitType,
double dbXValue, double dbYValue, double dbZValue, double dbRxValue, double dbRyValue, double dbRzValue, double dbAcc, double dbDec, double dbSpeed)
{
return(0);
}
public TextBySpeed()
{
text = "";
speedType = SpeedTypes.MEDIUM;
}
