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