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