RobotEquip_InExecute.cs
10.1 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
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
using CodeLibrary;
using OnlineStore.Common;
using OnlineStore.LoadCSVLibrary;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using System.Windows.Forms.VisualStyles;
namespace OnlineStore.DeviceLibrary
{
partial class RobotEquip
{
#region 入库
private void InstoreExecute()
{
switch (MoveInfo.MoveStep)
{
case StepEnum.InStore_01_Wait:
SetBoxStatus(DeviceStatus.InStoreExecute, RunStatus.Busy, MoveInfo.MoveParam.PosInfo);
NextMoveStep(StepEnum.InStore_02_ArmToP1, "入库开始，机械臂回待机点");
InStore_02_ArmToP1();
break;
case StepEnum.InStore_02_ArmToP1:
NextMoveStep(StepEnum.InStore_03_MoveAxisToInlet, "行走机构去入料口");
InStore_03_MoveAxisToInlet();
break;
case StepEnum.InStore_03_MoveAxisToInlet:
NextMoveStep(StepEnum.InStore_04_CheckInletState, "到达入料口，检查其状态");
break;
case StepEnum.InStore_04_CheckInletState:
if (InStore_04_CheckInletState())
{
NextMoveStep(StepEnum.InStore_05_ArmToTrayLow, $"机械臂去入料口的{MoveInfo.MoveParam.PosInfo.PlateW}寸料盘取料低点");
InStore_05_ArmToTrayLow(MoveInfo.MoveParam.PosInfo.PlateW);
}
break;
case StepEnum.InStore_05_ArmToTrayLow:
NextMoveStep(StepEnum.InStore_06_CompressTray, "夹紧料盘，机械臂到入料口取料高点");
InStore_06_CompressTray();
break;
case StepEnum.InStore_06_CompressTray:
NextMoveStep(StepEnum.InStore_07_ArmBackToP1, "在入料口取料完成，机械臂回待机点");
InStore_07_ArmBackToP1();
BufferDataManager.TrayInRobotInfo = MoveInfo.MoveParam.PosInfo.ToCopy();
break;
case StepEnum.InStore_07_ArmBackToP1:
if (CheckInstoreTrayTarget())
{
NextMoveStep(StepEnum.InStore_08_MoveAxisToStore, "清除入料口缓存,行走机构去目标仓位");
//在入料口取料完成
BufferDataManager.TrayInInletInfo = null;
UpdateLocInfo(MoveInfo.MoveParam.PosInfo, TaskStatus.INROBOT);
InStore_08_MoveAxisToStore();
}
else
{
NextMoveStep(StepEnum.InStore_12_MoveAxisToNGBox, "行走机构去NG箱");
//在入料口取料完成
BufferDataManager.TrayInInletInfo = null;
InStore_12_MoveAxisToNGBox();
}
break;
case StepEnum.InStore_08_MoveAxisToStore:
NextMoveStep(StepEnum.InStore_09_CheckStoreState, "行走机构到达指定仓，检查其状态");
break;
case StepEnum.InStore_09_CheckStoreState:
if (InStore_09_CheckStoreState(MoveInfo.MoveParam.PosInfo.Cid))
{
NextMoveStep(StepEnum.InStore_10_ArmToStoreDoorHigh, "当前仓门口可放料，机械臂到仓放料高点");
InStore_10_ArmToStoreDoorHigh();
}
break;
case StepEnum.InStore_10_ArmToStoreDoorHigh:
NextMoveStep(StepEnum.InStore_11_ReleaseTray, "机械臂到放料低点，压紧轴张开");
InStore_11_ReleaseTray();
break;
case StepEnum.InStore_11_ReleaseTray:
NextMoveStep(StepEnum.InStore_16_ArmBackToP1, "在仓门口放料完成，清除机器人料叉缓存，机械臂回待机点");
//在料仓口口放料完成
BufferDataManager.TrayInRobotInfo = null;
InStore_16_ArmBackToP1();
break;
#region 去NG箱
case StepEnum.InStore_12_MoveAxisToNGBox:
NextMoveStep(StepEnum.InStore_13_CheckNGBoxState, "检查NG箱状态");
break;
case StepEnum.InStore_13_CheckNGBoxState:
if (InStore_13_CheckNGBoxState())
{
NextMoveStep(StepEnum.InStore_14_ArmToNGBoxHigh, "NG箱可放料，机械臂去NG箱放料高点");
ClearSpecifiedAlarm($"NG箱中已存在");
InStore_14_ArmToNGBoxHigh();
}
else
{
SetWarnMsg($"NG箱中已存在{TrayCntInNGBox}个料盘，机器人无法放料，需要清空NG箱");
}
break;
case StepEnum.InStore_14_ArmToNGBoxHigh:
NextMoveStep(StepEnum.InStore_15_ReleaseTray, "机械臂到放料低点，压紧轴张开");
BufferDataManager.AddTrayInNGBox(BufferDataManager.TrayInRobotInfo.ToCopy());
MoveInfo.WaitList.Add(WaitResultInfo.WaitTime(1000));
InStore_15_ReleaseTray();
break;
case StepEnum.InStore_15_ReleaseTray:
NextMoveStep(StepEnum.InStore_16_ArmBackToP1, "在NG箱放料完成，清除机器人料叉缓存，机械臂回待机点");
//在料仓口口放料完成
BufferDataManager.TrayInRobotInfo = null;
// UpdateLocInfo(MoveInfo.MoveParam.PosInfo.barcode, TaskStatus.BOXDOOR);
InStore_16_ArmBackToP1();
break;
#endregion
case StepEnum.InStore_16_ArmBackToP1:
NextMoveStep(StepEnum.InStore_17_Finish, "入库结束");
if (CheckInstoreTrayTarget())
UpdateLocInfo(MoveInfo.MoveParam.PosInfo, TaskStatus.BOXDOOR);
InStore_17_Finish();
break;
case StepEnum.InStore_17_Finish:
runStatus = RunStatus.Runing;
MoveInfo.EndMove();
break;
}
}
/// <summary>
/// 检查入库料盘的目的地
/// </summary>
/// <returns>true，去料仓；false：去ng箱子</returns>
private bool CheckInstoreTrayTarget()
{
return !MoveInfo.MoveParam.PosInfo.IsNG;
}
private void InStore_02_ArmToP1()
{
SendCmd(GetCmd_Standby());
}
private void InStore_03_MoveAxisToInlet()
{
MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P2_Inlet, Config.MoveAxis_P2_Inlet_Speed);
}
private bool InStore_04_CheckInletState()
{
if (MoveInfo.MoveParam.PosInfo != null)
return true;
return false;
}
private void InStore_05_ArmToTrayLow(int width = 7)
{
if (width == 7 || width == 13)
SendCmd(GetCmd_InletLow(width));
else
SendCmd(GetCmd_InletLow(7));
}
private void InStore_06_CompressTray()
{
SendCmd(GetCmd_InletUp());
CompressAxis.AbsMove(MoveInfo, Config.CompressAxis_P4, Config.CompressAxis_P4_Speed);
MoveInfo.WaitList.Add(WaitResultInfo.WaitIO(IO_Type.RobotArm_ClampingSig, IO_VALUE.HIGH));
}
private void InStore_07_ArmBackToP1()
{
SendCmd(GetCmd_Standby());
}
private void InStore_08_MoveAxisToStore()
{
MoveAxisToStore();
}
/// <summary>
/// 检查当前仓门口是否可用
/// </summary>
/// <returns></returns>
private bool InStore_09_CheckStoreState(string cid)
{
return SServerManager.GetBoxDoorStatus(cid);
}
private void InStore_10_ArmToStoreDoorHigh()
{
SendCmd(GetCmd_StoreHigh(targetIndex));
}
private void InStore_11_ReleaseTray()
{
SendCmd(GetCmd_StoreDown());
CompressAxis.AbsMove(MoveInfo, Config.CompressAxis_P2, Config.CompressAxis_P2_Speed);
MoveInfo.WaitList.Add(WaitResultInfo.WaitIO(IO_Type.RobotArm_ClampingSig, IO_VALUE.LOW));
}
private void InStore_12_MoveAxisToNGBox()
{
MoveAxis.AbsMove(MoveInfo, Config.MoveAxis_P4_NGBox, Config.MoveAxis_P4_NGBox_Speed);
}
private bool InStore_13_CheckNGBoxState()
{
if (TrayCntInNGBox >= validityCnt)
return false;
return true;
}
private void InStore_14_ArmToNGBoxHigh()
{
SendCmd(GetCmd_NGBoxHigh());
}
private void InStore_15_ReleaseTray()
{
SendCmd(GetCmd_NGBoxDown());
CompressAxis.AbsMove(MoveInfo, Config.CompressAxis_P2, Config.CompressAxis_P2_Speed);
MoveInfo.WaitList.Add(WaitResultInfo.WaitIO(IO_Type.RobotArm_ClampingSig, IO_VALUE.LOW));
}
private void InStore_16_ArmBackToP1()
{
SendCmd(GetCmd_Standby());
}
private void InStore_17_Finish()
{
targetIndex = 0;
}
#endregion
/// <summary>
/// 机器人更新任务状态
/// </summary>
/// <param name="posinfo"></param>
/// <param name="status"></param>
private void UpdateLocInfo(InOutPosInfo posinfo, string status)
{
string result = SServerManager.UpdateTrayLoc(posinfo.barcode, status, posinfo.PosId, posinfo.taskId);
if (!result.Equals(""))
{
SServerManager.UpdateTrayLoc(posinfo.barcode, status, posinfo.PosId, posinfo.taskId);
}
}
}
}