|
@@ -31,12 +31,18 @@ namespace Venus_RT.Devices
|
|
|
PickRetract,
|
|
|
PlaceExtent,
|
|
|
PlaceRetract,
|
|
|
+ QueryAwc
|
|
|
}
|
|
|
|
|
|
private RState _status;
|
|
|
private bool _IsHomed;
|
|
|
public RState Status { get { return _status; } }
|
|
|
public bool IsHomed { get { return _IsHomed; } }
|
|
|
+ private double offset_x = 0;
|
|
|
+ private double offset_y = 0;
|
|
|
+ public double Offset_X => offset_x;
|
|
|
+ public double Offset_Y => offset_y;
|
|
|
+ public double Offset_D => Math.Round(Math.Sqrt(Math.Pow(offset_x, 2) + Math.Pow(offset_y, 2)), 2);//欧式距离 保留后两位
|
|
|
|
|
|
public RobotMoveInfo TMRobotMoveInfo { get { return _robotMoveInfo; } }
|
|
|
|
|
@@ -50,6 +56,10 @@ namespace Venus_RT.Devices
|
|
|
|
|
|
private readonly Regex _rex_check_load = new Regex(@"LOAD\s+(A|B)\s+(\w+)\s*");
|
|
|
private readonly Regex _rex_error_code = new Regex(@"_ERR\s+(\d+)\s*");
|
|
|
+ private readonly Regex _rex_event_offset = new Regex(@"_EVENT\sROBOT\s[0-9|\s]*.*");
|
|
|
+ private readonly Regex _rex_event_getoffset = new Regex(@"(?<=_EVENT\sROBOT\s)(.+?)(?=\sB)");
|
|
|
+ private readonly Regex _rex_query_awc = new Regex(@"WAF_CEN\sRT[-|0-9|\s]*LFT[-|0-9|\s]*OFFSET[-|0-9|\s]*");
|
|
|
+ private readonly Regex _rex_query_getoffset = new Regex(@"(?<=WAF_CEN\sRT[-|0-9|\s]*LFT[-|0-9|\s]*OFFSET\s)(.*)");
|
|
|
|
|
|
private const string EOF = "\r\n";
|
|
|
|
|
@@ -95,6 +105,19 @@ namespace Venus_RT.Devices
|
|
|
_status = RState.Running;
|
|
|
return _SendCommand($"CHECK LOAD {_checkLoadStation} ARM {Hand2Arm(hand)}");
|
|
|
}
|
|
|
+
|
|
|
+ public bool QueryAwc()
|
|
|
+ {
|
|
|
+ //检查防止状态交叉
|
|
|
+ if (CheckRobotStatus() == false)
|
|
|
+ return false;
|
|
|
+ offset_x = 0;
|
|
|
+ offset_y = 0;
|
|
|
+ _currentOP = OPStep.QueryAwc;
|
|
|
+ _status = RState.Running;
|
|
|
+ return _SendCommand("RQ WAF_CEN DATA");
|
|
|
+ }
|
|
|
+
|
|
|
public bool Goto(ModuleName station, int slot, Hand hand)
|
|
|
{
|
|
|
if (CheckRobotStatus() == false)
|
|
@@ -227,15 +250,19 @@ namespace Venus_RT.Devices
|
|
|
case OPStep.PlaceExtent:
|
|
|
case OPStep.PlaceRetract:
|
|
|
{
|
|
|
- if (RevMsg.Trim() == "_RDY" || ( RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
|
|
|
+ if (RevMsg.Trim() == "_RDY" || (RevMsg.Contains("_RDY") && !RevMsg.Contains("_ERR")))
|
|
|
{
|
|
|
_currentOP = OPStep.Idle;
|
|
|
_status = RState.End;
|
|
|
+ if (RevMsg.Contains("_EVENT"))
|
|
|
+ {
|
|
|
+ GetEventMsg(RevMsg);
|
|
|
+ }
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
ReportWrongMsg(RevMsg);
|
|
|
- }
|
|
|
+ }
|
|
|
|
|
|
if (_currentOP != OPStep.PickExtend && _currentOP != OPStep.PlaceExtent)
|
|
|
SetRobotMovingInfo(RobotAction.None, Hand.Both, ModuleName.TMRobot);
|
|
@@ -279,9 +306,44 @@ namespace Venus_RT.Devices
|
|
|
}
|
|
|
}
|
|
|
break;
|
|
|
+ case OPStep.QueryAwc:
|
|
|
+ {
|
|
|
+ //不沾包
|
|
|
+ if (RevMsg.Trim() == "_RDY")
|
|
|
+ {
|
|
|
+ _currentOP = OPStep.Idle;
|
|
|
+ _status = RState.End;
|
|
|
+ }
|
|
|
+ //单条查询处理
|
|
|
+ GetAwcMsg(RevMsg);
|
|
|
+ //沾包
|
|
|
+ if (RevMsg.Trim() != "_RDY" && RevMsg.Contains("_RDY"))
|
|
|
+ {
|
|
|
+ if (!RevMsg.Contains("_ERR"))
|
|
|
+ {
|
|
|
+ foreach (string msg in RevMsg.Split('\n'))
|
|
|
+ GetAwcMsg(msg);
|
|
|
+
|
|
|
+ _currentOP = OPStep.Idle;
|
|
|
+ _status = RState.End;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ foreach (string msg in RevMsg.Split('\n'))
|
|
|
+ if (msg.Contains("_ERR"))
|
|
|
+ ErrorMessageHandler(_rex_error_code.Match(RevMsg.Trim()).Value);
|
|
|
+
|
|
|
+ _currentOP = OPStep.Idle;
|
|
|
+ _status = RState.Failed;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+ break;
|
|
|
default:
|
|
|
if (!RevMsg.Contains("_EVENT"))
|
|
|
ReportWrongMsg(RevMsg);
|
|
|
+ else
|
|
|
+ GetEventMsg(RevMsg);
|
|
|
break;
|
|
|
}
|
|
|
}
|
|
@@ -612,5 +674,53 @@ namespace Venus_RT.Devices
|
|
|
_robotMoveInfo.ArmTarget = hand == Hand.Blade1 ? RobotArm.ArmA : (hand == Hand.Both ? RobotArm.Both : RobotArm.ArmB);
|
|
|
_robotMoveInfo.BladeTarget = $"{_robotMoveInfo.ArmTarget}.{target}";
|
|
|
}
|
|
|
+
|
|
|
+ private void GetEventMsg(string revMsg)
|
|
|
+ {
|
|
|
+ //在包含数据的前提下
|
|
|
+ switch (_robotMoveInfo.Action)
|
|
|
+ {
|
|
|
+ case RobotAction.Picking:
|
|
|
+ case RobotAction.Placing:
|
|
|
+ case RobotAction.Extending:
|
|
|
+ case RobotAction.Retracting:
|
|
|
+ if (_rex_event_offset.IsMatch(revMsg))
|
|
|
+ {
|
|
|
+ //offset_x = _rex_event_getoffset.Match(revMsg).Value.Split(' ')[0];
|
|
|
+ //offset_y = _rex_event_getoffset.Match(revMsg).Value.Split(' ')[1];
|
|
|
+ }
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+ private void GetAwcMsg(string revMsg)
|
|
|
+ {
|
|
|
+ revMsg = revMsg.Trim();
|
|
|
+ if (_rex_query_awc.IsMatch(revMsg))
|
|
|
+ {
|
|
|
+ string offset_r_t = _rex_query_getoffset.Match(revMsg).Value;
|
|
|
+ //最大仅6位 不超过int范围
|
|
|
+ int offset_r;
|
|
|
+ int offset_t;
|
|
|
+ if (int.TryParse(offset_r_t.Split(' ')[0], out offset_r) && int.TryParse(offset_r_t.Split(' ')[1], out offset_t))
|
|
|
+ {
|
|
|
+ // 9/26 新松暂未提供转换公式 暂时使用相关数据
|
|
|
+ offset_x = offset_r * Math.Cos(offset_t);
|
|
|
+ offset_y = offset_t * Math.Sin(offset_t);
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"TM Robot returned illegal offset data! Raw Data:{revMsg}");
|
|
|
+ }
|
|
|
+ }
|
|
|
+ //else
|
|
|
+ //{
|
|
|
+ // LOG.Write(eEvent.ERR_TM_ROBOT, ModuleName.TMRobot, $"The awc parameter format returned by TM Robot is incorrect! Raw Data:{revMsg}");
|
|
|
+ //}
|
|
|
+ }
|
|
|
}
|
|
|
}
|