Skip to content

孙克 / URSolderingRobot

转到一个项目
切换导航栏
切换导航栏固定状态
Commit b2aabcfe 由 几米阳光
选项

机器人调试修改

1 个父辈 cffa60e0
内嵌 并排
正在显示 2 个修改的文件 包含 171 行增加 和 100 行删除
DeviceLibrary/deviceLibrary/urRobot/URRobotClient.cs
...@@ -10,6 +10,10 @@ namespace URSoldering.DeviceLibrary ...@@ -10,6 +10,10 @@ namespace URSoldering.DeviceLibrary
{ {
public class URRobotClient public class URRobotClient
{ {
public delegate void SafetModePro(string mode);
public static event SafetModePro SafetModeFun;
public delegate void RobotModePro(string mode);
public static event RobotModePro RobotModeFun;
private static URTcpClient listenClient = null; private static URTcpClient listenClient = null;
private static int Port = 30003; private static int Port = 30003;
public static string LastMoveCMD = ""; public static string LastMoveCMD = "";
...@@ -88,9 +92,10 @@ namespace URSoldering.DeviceLibrary ...@@ -88,9 +92,10 @@ namespace URSoldering.DeviceLibrary
doubleList.Add(messageSize); doubleList.Add(messageSize);
if (maxdouble > 61) if (maxdouble > 61)
{ {
int[] array = new int[] { 55,56,57,58,59,60,94,101,103,132};
for (int i = 0; i < maxdouble; i++) for (int i = 0; i < maxdouble; i++)
{ {
if (i >= 55 && i <= 61) if (array.Contains(i))
{ {
if (reviceData.Length >= (12 + i * 8)) if (reviceData.Length >= (12 + i * 8))
{ {
...@@ -118,11 +123,16 @@ namespace URSoldering.DeviceLibrary ...@@ -118,11 +123,16 @@ namespace URSoldering.DeviceLibrary
double ry = doubleList[60] * 1; double ry = doubleList[60] * 1;
double rz = doubleList[61] * 1; double rz = doubleList[61] * 1;
URPointValue newp = new URPointValue(x, y, z, rx, ry, rz); URPointValue newp = new URPointValue(x, y, z, rx, ry, rz);
int robotMode =(int) doubleList[95];
//string reviceMsg = ByteToStr(reviceData); int safetyMode =(int) doubleList[102];
string robotModeStr = GetRobotMode(robotMode);
string safetyModeStr = GetSafetyMode(safetyMode);
int programState = (int)doubleList[132];
SafetModeFun?.Invoke(safetyModeStr);
RobotModeFun?.Invoke(robotModeStr);
if (!URRobotControl.IsSamePoint(newp, URRobotControl.LastPoint)) if (!URRobotControl.IsSamePoint(newp, URRobotControl.LastPoint))
{ {
LogUtil.URLInfo(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]坐标" + newp.ToShowStr()); LogUtil.URLInfo(LogName + "length[" + messageSize + "]，data长[" + reviceData.Length + "]，【" + programState + "】【" + robotModeStr + "】【" + safetyModeStr + "】坐标" + newp.ToShowStr()+"");
} }
URRobotControl.LastPoint = newp; URRobotControl.LastPoint = newp;
} }
...@@ -139,6 +149,57 @@ namespace URSoldering.DeviceLibrary ...@@ -139,6 +149,57 @@ namespace URSoldering.DeviceLibrary
StopListen(); StopListen();
} }
} }
public static string GetRobotMode(int type)
{
switch (type)
{
case 0:
return "ROBOT_MODE_DISCONNECTED";
case 1:
return "ROBOT_MODE_CONFIRM_SAFETY";
case 2:
return "ROBOT_MODE_BOOTING";
case 3:
return "ROBOT_MODE_POWER_OFF";
case 4:
return "ROBOT_MODE_POWER_ON";
case 5:
return "ROBOT_MODE_IDLE";
case 6:
return "ROBOT_MODE_BACKDRIVE";
case 7:
return "ROBOT_MODE_RUNNING";
case 8:
return "ROBOT_MODE_UPDATING_FIRMWARE";
default:
return "";
}
}
public static string GetSafetyMode(int type)
{
switch (type)
{
case 1:
return "SAFETY_MODE_NORMAL";
case 2:
return "SAFETY_MODE_REDUCED";
case 3:
return "SAFETY_MODE_PROTECTIVE_STOP";
case 4:
return "SAFETY_MODE_RECOVERY";
case 5:
return "SAFETY_MODE_SAFEGUARD_STOP";
case 6:
return "SAFETY_MODE_SYSTEM_EMERGENCY_STOP";
case 7:
return "SAFETY_MODE_ROBOT_EMERGENCY_STOP";
case 8:
return "SAFETY_MODE_VIOLATION";
case 9:
return "SAFETY_MODE_FAULT";
default:
return "";
}
}
} }
} }
DeviceLibrary/deviceLibrary/urRobot/URRobotControl.cs
...@@ -62,6 +62,8 @@ namespace URSoldering.DeviceLibrary ...@@ -62,6 +62,8 @@ namespace URSoldering.DeviceLibrary
public static void InitConfig(string ip) public static void InitConfig(string ip)
{ {
RobotIp = ip; RobotIp = ip;
URRobotClient.SafetModeFun += SafetyEProcess;
URRobotClient.RobotModeFun += ModeEProcess;
} }
public static string LogName public static string LogName
{ {
...@@ -107,7 +109,7 @@ namespace URSoldering.DeviceLibrary ...@@ -107,7 +109,7 @@ namespace URSoldering.DeviceLibrary
{ {
URWithOutTP a = new URWithOutTP(); //创建对象 URWithOutTP a = new URWithOutTP(); //创建对象
a.restartURControl(RobotIp); //重置UR控制器，然后可以利用dashboard，poweron,brake release a.restartURControl(RobotIp); //重置UR控制器，然后可以利用dashboard，poweron,brake release
LogUtil.info(LogName+"重置完成"); LogUtil.info(LogName + "重置完成");
Thread.Sleep(100); Thread.Sleep(100);
} }
/// <summary> /// <summary>
...@@ -197,7 +199,7 @@ namespace URSoldering.DeviceLibrary ...@@ -197,7 +199,7 @@ namespace URSoldering.DeviceLibrary
IsInPorcess = true; IsInPorcess = true;
try try
{ {
if (IsRun&& IsStartConnect) if (IsRun && IsStartConnect)
{ {
TimeSpan span = DateTime.Now - preCheckTime; TimeSpan span = DateTime.Now - preCheckTime;
if (span.TotalSeconds > 2) if (span.TotalSeconds > 2)
...@@ -206,7 +208,7 @@ namespace URSoldering.DeviceLibrary ...@@ -206,7 +208,7 @@ namespace URSoldering.DeviceLibrary
//判断500在连接上，则获取急停状态 //判断500在连接上，则获取急停状态
if (controlTcp.IsConnected()) if (controlTcp.IsConnected())
{ {
controlTcp.sendLine(CMD_Safetymode); //controlTcp.sendLine(CMD_Safetymode);
} }
else else
{ {
...@@ -255,12 +257,8 @@ namespace URSoldering.DeviceLibrary ...@@ -255,12 +257,8 @@ namespace URSoldering.DeviceLibrary
} }
return true; return true;
} }
public static bool GetRobotMode()
{
return SendCMD(CMD_robotMode, 0);
}
private static void OnControlRevice(string message) private static void OnControlRevice(string message)
{ {
if (message == null || message.Equals("")) if (message == null || message.Equals(""))
...@@ -285,11 +283,11 @@ namespace URSoldering.DeviceLibrary ...@@ -285,11 +283,11 @@ namespace URSoldering.DeviceLibrary
} }
else if (message.ToLower().IndexOf(REV_RobotMode) >= 0) else if (message.ToLower().IndexOf(REV_RobotMode) >= 0)
{ {
StatusProcess(message); ModeProcess(message);
} }
else if (message.ToLower().IndexOf(REV_SafetyMode) >= 0) else if (message.ToLower().IndexOf(REV_SafetyMode) >= 0)
{ {
SafetymodeProcess(message); SafetyProcess(message);
} }
else else
{ {
...@@ -301,97 +299,106 @@ namespace URSoldering.DeviceLibrary ...@@ -301,97 +299,106 @@ namespace URSoldering.DeviceLibrary
LogUtil.URSError(LogName + " OnControlRevice出错啦" + ex.ToString()); LogUtil.URSError(LogName + " OnControlRevice出错啦" + ex.ToString());
} }
} }
private static void StatusProcess(string message) private static void ModeProcess(string message)
{ {
string msg = message.ToLower().Replace(REV_RobotMode + ": ", "").ToUpper().Trim(); string msg = message.ToLower().Replace(REV_RobotMode + ": ", "").ToUpper().Trim();
if (!IsRun) if (msg.Equals(URStatus.POWER_ON))
{ {
if (msg.Equals(URStatus.POWER_ON)) SendCMD(CMD_brakeRelease, 1000);
{ }
SendCMD(CMD_brakeRelease, 1000); else if (msg.Equals(URStatus.RUNNING) || msg.Equals(URStatus.IDLE))
} {
else if (msg.Equals(URStatus.RUNNING) || msg.Equals(URStatus.IDLE)) WarnMsg = "";
{
WarnMsg = "";
LogUtil.URSInfo(LogName + " 当前状态：" + msg + "，机器人连接成功！");
LogUtil.info(LogName + " 当前状态：" + msg + "，机器人连接成功！");
IsRun = true;
reconnectTimer.Enabled = true;
} LogUtil.URSInfo(LogName + " 当前状态：" + msg + "，机器人连接成功！");
else LogUtil.info(LogName + " 当前状态：" + msg + "，机器人连接成功！");
{ IsRun = true;
if (!IsTimeOut(message)) reconnectTimer.Enabled = true;
{
LogUtil.URSInfo(LogName + "当前状态：" + msg + "，发送打开命令" + CMD_powerOn);
SendCMD(CMD_powerOn, 1000);
}
else
{
WarnMsg = "机器人状态[" + msg + "]，启动超时";
StopRobot();
}
}
} }
else else
{ {
if (msg.Equals(URStatus.POWER_ON)) if (!IsTimeOut(message))
{
SendCMD(CMD_brakeRelease, 1000);
}
else if (msg.Equals(URStatus.RUNNING) || msg.Equals(URStatus.IDLE))
{ {
LogUtil.URSInfo(LogName + "当前状态：" + msg + "，发送打开命令" + CMD_powerOn);
SendCMD(CMD_powerOn, 1000);
} }
else else
{ {
WarnMsg = "机器人状态[" + msg + "]，尝试自动复位"; WarnMsg = "机器人状态[" + msg + "]，启动超时";
LogUtil.info(LogName + WarnMsg); StopRobot();
IsRun = false;
IsStartConnect = true;
PreStartTime = DateTime.Now;
SendCMD(CMD_powerOn, 500);
} }
} }
} }
private static void SafetymodeProcess(string message)
private static void SafetyProcess(string message)
{ {
string msg = message.ToLower().Replace(REV_SafetyMode + ": ", "").ToUpper().Trim(); string msg = message.ToLower().Replace(REV_SafetyMode + ": ", "").ToUpper().Trim();
if (!IsRun)
if (msg.Equals(URStatus.SFETY_POWER_OFF) || msg.Equals(URStatus.ROBOT_EMERGENCY_STOP))
{ {
if (msg.Equals(URStatus.SFETY_POWER_OFF) || msg.Equals(URStatus.ROBOT_EMERGENCY_STOP)) if (!IsTimeOut(message))
{ {
if (!IsTimeOut(message)) WarnMsg = "机器人急停中[" + msg + "]，请先打开急停";
{ SendCMD(CMD_Safetymode, 1000);
WarnMsg = "机器人急停中[" + msg + "]，请先打开急停";
SendCMD(CMD_Safetymode, 1000);
}
else
{
WarnMsg = "机器人急停中[" + msg + "]，启动超时";
StopRobot();
}
} }
else else
{ {
SendCMD(CMD_robotMode, 500); WarnMsg = "机器人急停中[" + msg + "]，启动超时";
}
}else
{
if (msg.Equals(URStatus.SFETY_POWER_OFF))
{
WarnMsg = "机器人急停中[" + msg + "]，请复位";
LogUtil.info(LogName + WarnMsg);
StopRobot();
} else if (msg.Equals(URStatus.ROBOT_EMERGENCY_STOP)){
WarnMsg = "机器人急停中[" + msg + "]，请复位";
LogUtil.info(LogName + WarnMsg);
StopRobot(); StopRobot();
} }
else }
{ else
SendCMD(CMD_robotMode, 500); {
} SendCMD(CMD_robotMode, 500);
}
}
private static void ModeEProcess(string mode)
{
if (!IsRun)
{
return;
}
mode = mode.Replace("ROBOT_MODE_", "").ToUpper().Trim();
if (mode.Equals(URStatus.POWER_ON))
{
SendCMD(CMD_brakeRelease, 1000);
}
else if (mode.Equals(URStatus.RUNNING) || mode.Equals(URStatus.IDLE))
{
}
else
{
WarnMsg = "机器人状态[" + mode + "]，尝试自动复位";
LogUtil.info(LogName + WarnMsg);
IsRun = false;
IsStartConnect = true;
PreStartTime = DateTime.Now;
SendCMD(CMD_powerOn, 500);
}
}
private static void SafetyEProcess(string safetyStatus)
{
if (!IsRun)
{
return;
}
safetyStatus = safetyStatus.Replace("SAFETY_MODE_", "").ToUpper().Trim();
if (safetyStatus.Equals(URStatus.SFETY_POWER_OFF))
{
WarnMsg = "机器人急停中[" + safetyStatus + "]，请复位";
LogUtil.info(LogName + WarnMsg);
StopRobot();
}
else if (safetyStatus.Equals(URStatus.ROBOT_EMERGENCY_STOP))
{
WarnMsg = "机器人急停中[" + safetyStatus + "]，请复位";
LogUtil.info(LogName + WarnMsg);
StopRobot();
}
else
{
// SendCMD(CMD_robotMode, 500);
} }
} }
private static bool IsTimeOut(string status) private static bool IsTimeOut(string status)
...@@ -402,11 +409,11 @@ namespace URSoldering.DeviceLibrary ...@@ -402,11 +409,11 @@ namespace URSoldering.DeviceLibrary
string logMsg = LogName + "【" + status + "】 已持续" + Math.Round(span.TotalMilliseconds) + "，启动超时！"; string logMsg = LogName + "【" + status + "】 已持续" + Math.Round(span.TotalMilliseconds) + "，启动超时！";
LogUtil.info(logMsg); LogUtil.info(logMsg);
LogUtil.URSInfo(logMsg); LogUtil.URSInfo(logMsg);
return true ; return true;
} }
return false ; return false;
} }
private static void stopTcp() private static void stopTcp()
{ {
...@@ -466,11 +473,11 @@ namespace URSoldering.DeviceLibrary ...@@ -466,11 +473,11 @@ namespace URSoldering.DeviceLibrary
{ {
if (!PointIsValid(point)) if (!PointIsValid(point))
{ {
LogUtil.info(LogName + "试图移动到：" + point.ToShowStr()+",此坐标无效，直接返回"); LogUtil.info(LogName + "试图移动到：" + point.ToShowStr() + ",此坐标无效，直接返回");
return; return;
} }
//movep(p[0.062, -0.030, 0.325, 0.0299, 2.2263, 2.2171], a = 1.2, v = 0.25, r = 0) //movep(p[0.062, -0.030, 0.325, 0.0299, 2.2263, 2.2171], a = 1.2, v = 0.25, r = 0)
//movej([0, 1.57, -1.57, 3.14, -1.57, 1.57],a = 1.4, v = 1.05, t = 0, r = 0) //movej([0, 1.57, -1.57, 3.14, -1.57, 1.57],a = 1.4, v = 1.05, t = 0, r = 0)
string x = Math.Round(point.X / 1000F, 5).ToString(); string x = Math.Round(point.X / 1000F, 5).ToString();
string y = Math.Round(point.Y / 1000F, 5).ToString(); string y = Math.Round(point.Y / 1000F, 5).ToString();
...@@ -482,7 +489,7 @@ namespace URSoldering.DeviceLibrary ...@@ -482,7 +489,7 @@ namespace URSoldering.DeviceLibrary
moveCmd = "movep(p[" + x + spiltStr + y + spiltStr + z + spiltStr + point.RX + spiltStr + point.RY + spiltStr + point.RZ + "],a=1.0, v=1.0, r=0)"; moveCmd = "movep(p[" + x + spiltStr + y + spiltStr + z + spiltStr + point.RX + spiltStr + point.RY + spiltStr + point.RZ + "],a=1.0, v=1.0, r=0)";
} }
ClearPreCMD(); ClearPreCMD();
URRobotClient.LastMoveCMD = moveCmd; URRobotClient.LastMoveCMD = moveCmd;
LastSendPoint = point; LastSendPoint = point;
} }
...@@ -490,15 +497,15 @@ namespace URSoldering.DeviceLibrary ...@@ -490,15 +497,15 @@ namespace URSoldering.DeviceLibrary
{ {
if (!URRobotClient.LastMoveCMD.Equals("")) if (!URRobotClient.LastMoveCMD.Equals(""))
{ {
LogUtil.info(LogName + ",清除上次未发送指令：【" + URRobotClient.LastMoveCMD + "】" ); LogUtil.info(LogName + ",清除上次未发送指令：【" + URRobotClient.LastMoveCMD + "】");
URRobotClient.LastMoveCMD = ""; URRobotClient.LastMoveCMD = "";
} }
} }
public static void MoveTo(URPointValue point) public static void MoveTo(URPointValue point)
{ {
MoveTo(point, false); MoveTo(point, false);
} }
public static bool MoveOK(URPointValue point,DateTime moveTime) public static bool MoveOK(URPointValue point, DateTime moveTime)
{ {
URPointValue lastPoint = GetLastPosition(); URPointValue lastPoint = GetLastPosition();
if (lastPoint.UpdateTime > moveTime) if (lastPoint.UpdateTime > moveTime)
...@@ -507,9 +514,10 @@ namespace URSoldering.DeviceLibrary ...@@ -507,9 +514,10 @@ namespace URSoldering.DeviceLibrary
{ {
return true; return true;
} }
}return false; }
return false;
} }
public static bool IsSamePoint(URPointValue p1,URPointValue p2) public static bool IsSamePoint(URPointValue p1, URPointValue p2)
{ {
if (p1 == null && p2 == null) if (p1 == null && p2 == null)
{ {
...@@ -527,14 +535,12 @@ namespace URSoldering.DeviceLibrary ...@@ -527,14 +535,12 @@ namespace URSoldering.DeviceLibrary
} }
return false; return false;
} }
/// <summary>
/// 获取机器人最后一次获取的坐标
/// </summary>
/// <returns></returns>
public static URPointValue GetLastPosition() public static URPointValue GetLastPosition()
{ {
return LastPoint; return LastPoint;
} }
} }
public class URPointValue public class URPointValue
{ {
...@@ -585,6 +591,8 @@ namespace URSoldering.DeviceLibrary ...@@ -585,6 +591,8 @@ namespace URSoldering.DeviceLibrary
{ {
return JsonHelper.DeserializeJsonToObject<URPointValue>(json); return JsonHelper.DeserializeJsonToObject<URPointValue>(json);
} }
}
} }
public struct URStatus public struct URStatus
{ {
...@@ -610,4 +618,6 @@ namespace URSoldering.DeviceLibrary ...@@ -610,4 +618,6 @@ namespace URSoldering.DeviceLibrary
/// </summary> /// </summary>
public static string SFETY_POWER_OFF = "POWER_OFF"; public static string SFETY_POWER_OFF = "POWER_OFF";
} }
}
支持 Markdown 格式
你添加了 0 人 到此讨论。请谨慎行事。
Finish editing this message first!