RobotMoveHelper.cs
5.9 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
using OnlineStore;
using OnlineStore.Common;
using RemoteSheardObject;
using Robot.UR;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
namespace DeviceLibrary
{
public class RobotHelper
{
int lastMoveCmd = 0;
int lastSpeedRate = 0;
public int lastWeight = 0;
URRobotControl robot;
System.Timers.Timer RobotCheck;
string Robotname;
public RobotHelper(URRobotControl _robot, string robotname) {
robot = _robot;
Robotname = robotname;
RobotCheck = new System.Timers.Timer(5000);
RobotCheck.Elapsed += RobotCheck_Elapsed;
}
/// <summary>
/// 机器人是否可用
/// </summary>
public volatile bool RobotStatus = false;
/// <summary>
/// 机器人急停中
/// </summary>
public volatile bool EMERGENCY_STOP = false;
private void RobotCheck_Elapsed(object sender, System.Timers.ElapsedEventArgs e)
{
if (!robot.IsRun)
{
robot.StartRobot();
RobotStatus = false;
}
robot.SendCMD("safetymode", 0);
Thread.Sleep(100);
if (!robot.CurDashboardReponse.Contains("NORMAL"))
{
RobotStatus = false;
//Msg.add(POS_Start + "机器人状态异常:" + Robot.CurDashboardReponse, MsgLevel.alarm);
if (robot.CurDashboardReponse.Contains("PROTECTIVE_STOP"))
{
robot.SendCMD("unlock protective stop", 0);
lastWeight = lastWeight + 1;
if (lastWeight > 3)
lastWeight = 1;
robot.log("触发保护性停止,调节负载重量:" + lastWeight);
return;
}
else if (robot.CurDashboardReponse.Contains("EMERGENCY_STOP"))
{
robot.log("机器人急停中");
EMERGENCY_STOP = true;
}
return;
}
robot.SendCMD("programState", 0);
Thread.Sleep(700);
if (!robot.CurDashboardReponse.Contains("PLAYING"))
{
RobotStatus = false;
//Msg.add(POS_Start + "机器人状态异常:" + Robot.CurDashboardReponse, MsgLevel.alarm);
robot.StopProgram();
}
else if (!robot.ClientIsConnected)
{
RobotStatus = false;
robot.StopProgram();
Thread.Sleep(300);
robot.StopRobot();
}
else
{
RobotStatus = true;
EMERGENCY_STOP = false;
}
uploadStatus();
}
void uploadStatus() {
EquipMsgData equipMsg = new EquipMsgData();
equipMsg.msgList = new List<EquipMessage>();
equipMsg.status = RobotStatus ? 1 : 2;
if (!RobotStatus)
{
equipMsg.msgList.Add(new EquipMessage()
{
msg = crc.GetString("Res0058", "机器人当前不可用"),
type = EquipMessage.ConvertType(MsgLevel.alarm.ToString())
});
}
equipMsg.equipName = Robotname;
TheLine.UploadStatus(equipMsg);
}
DateTime LastMoveTime= DateTime.Now;
int retrytime = 0;
public void Move(MoveInfo moveInfo, int movecmd, int weight) {
if (moveInfo != null)
{
retrytime = 0;
LastMoveTime = DateTime.Now;
moveInfo.WaitList.Add(WaitResultInfo.WaitAction(new Func<WaitResultInfo, bool>(IsMoveOk), crc.GetString("Res0157", "等待") + $"[{robot.Name}]" + crc.GetString("Res0057", "移动到位")));
}
lastMoveCmd = movecmd;
lastSpeedRate = Setting_Init.URRobot_MI1_Speed_Rate;
lastWeight = weight;
robot.SendMoveCmd(movecmd, lastSpeedRate, false, LoadRateParam[weight]);
}
internal void Start()
{
RobotCheck.Enabled = true;
}
public bool IsMoveOk(WaitResultInfo waitResultInfo) {
//if (robot.CurCmdReponse.Contains($"{lastMoveCmd},done"))
if (robot.CurCmdReponse.StartsWith(lastMoveCmd.ToString()) && robot.CurCmdReponse.EndsWith("done"))
return true;
else if (robot.CurCmdReponse.Contains($"done") || retrytime>1)
{
retrytime = 0;
robot.log($"机器人没有移动到位,重新移动:{lastMoveCmd},{lastWeight},{robot.CurCmdReponse}");
robot.SendMoveCmd(lastMoveCmd, lastSpeedRate, false, LoadRateParam[lastWeight]);
return false;
}
else if ((DateTime.Now - LastMoveTime).TotalSeconds>30) {
retrytime++;
LastMoveTime = DateTime.Now;
robot.log($"机器人超过30秒没有反馈,停止程序重试");
robot.StopProgram();
return false;
}
else
return false;
}
internal void Stop()
{
RobotCheck.Enabled = false;
}
/// <summary>
/// key=load kg, value = load 公斤,x,y,z(米)
/// </summary>
public static Dictionary<int, float[]> LoadRateParam = new Dictionary<int, float[]>() {
{ 0, new float[] { 4.5f, 0, 37 / 1000f, 100 / 1000f } },
{ 1, new float[] { 5.5f, 0, 37 / 1000f, 146 / 1000f } },
{ 2, new float[] { 6.5f, 0, 37 / 1000f, 174 / 1000f } },
{ 3, new float[] { 7.5f, 0, 37 / 1000f, 204 / 1000f } },
{ 4, new float[] { 8.5f, 0, 37 / 1000f, 212 / 1000f } },
{ 5, new float[] { 9.5f, 0, 37 / 1000f, 224 / 1000f } }
};
}
}