Skip to content

孙克 / URSolderingRobot

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

机器人调试修改

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