public bool FindHeaderIsCloseRiskArea(Point p)
{
// return ExtensionService.CalDistance(TopHeader(),p)<properties.DistanceIntersection || ExtensionService.CalDistance(BottomHeader(), p) < properties.DistanceIntersection || ExtensionService.CalDistance(MiddleHeader(), p) < properties.DistanceIntersection ? true:false;
//Console.WriteLine("Vi tri robot "+ this.properties.NameID+" = " + properties.pose.Position);
// Console.WriteLine("Vi tien gan " + p.ToString());
// Console.WriteLine("kHOAN CACH " + ExtensionService.CalDistance(properties.pose.Position, p));
return(ExtensionService.CalDistance(properties.pose.Position, p) < properties.DistInter ? true : false);
}
protected bool checkRobotToFrontLineDistanceCtrl(RobotUnity rThis, List <RobotUnity> rCompList, Point frontLine)
{
double distRThisMin = ExtensionService.CalDistance(rThis.properties.pose.Position, frontLine);
foreach (RobotUnity rComp in rCompList)
{
double distRComp = ExtensionService.CalDistance(rComp.properties.pose.Position, frontLine);
if (distRThisMin > distRComp)
{
return(false);
}
}
return(true);
}
public int CheckSafeDistance() // KIểm tra khoản cách an toàn/ nếu đang trong vùng close với robot khác thì giảm tốc độ, chuyển sang chế độ dò risk area
{
int iscloseDistance = 0;
foreach (RobotUnity r in RobotUnitylist)
{
if (r.onFlagSupervisorTraffic)
{
Point rP = MiddleHeaderCv();
// bool onFound = r.FindHeaderIsCloseRiskArea(this.properties.pose.Position);
bool onFound = r.FindHeaderIsCloseRiskAreaCv(rP);
if (onFound)
{
// if robot in list is near but add in risk list robot
// robotLogOut.ShowTextTraffic(r.properties.Label + "- Intersection");
if (!RobotUnityRiskList.ContainsKey(r.properties.NameId))
{
RobotUnityRiskList.Add(r.properties.NameId, r);
}
// reduce speed robot control
iscloseDistance = 2;
}
else
{
// if robot in list is far but before registe in list, must remove in list
RemoveRiskList(r.properties.NameId);
double rd = ExtensionService.CalDistance(Global_Object.CoorCanvas(this.properties.pose.Position), Global_Object.CoorCanvas(r.properties.pose.Position));
if (rd < DistanceToSetSlowDown && rd > 60)
{
iscloseDistance = 1;
}
else
{
iscloseDistance = 0;
}
}
}
}
return(iscloseDistance);
}
protected bool TrafficCheckInBuffer(Pose frontLinePoint, int bayId)
{
if (ExtensionService.CalDistance(robot.properties.pose.Position, frontLinePoint.Position) < DISTANCE_CHECk_BAYID)
{
/* if (robot.bayId < 0)
* {
* robot.bayId = bayId;
* }*/
List <RobotUnity> rCompList = checkAllRobotsHasInsideBayIdNear(bayId, 2);
if (rCompList == null) // đã có 1 robot đã đăng ký thành công bayid
{
robot.SetSpeedHighPrioprity(RobotSpeedLevel.ROBOT_SPEED_STOP, true);
return(true);// tiep tuc check
}
if (rCompList.Count > 0)
{
// so sanh vi tri robot voi robot con lai
if (checkRobotToFrontLineDistanceCtrl(robot, rCompList, frontLinePoint.Position))
{
robot.bayIdReg = true;
robot.SetSpeedHighPrioprity(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
return(false); // ket thuc check
}
else
{
robot.SetSpeedHighPrioprity(RobotSpeedLevel.ROBOT_SPEED_STOP, true);
return(true); // tiep tuc check
}
}
else
{
robot.bayIdReg = true;
robot.SetSpeedHighPrioprity(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
return(false);// tiep tuc check
}
}
else
{
robot.SetSpeedHighPrioprity(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
return(false);// tiep tuc check
}
}