// Từ Elevator(C5) -> Ready and Outer
        // Detect no Robot : ready, GATE12
        public static bool Reg_checkinElevator_ReadyAndOuter(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegReady = false;
            bool onRegG12   = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("READY", robot) || traffic.HasOtherRobotUnityinArea("GATE12", robot))
            {
                return(false);
            }
            if (RegIntZone_READY.ProcessRegistryIntersectionZone(robot))
            {
                onRegReady = true;
            }
            if (RegIntZone_GATE12.ProcessRegistryIntersectionZone(robot))
            {
                onRegG12 = true;
            }
            if (onRegG12 && onRegReady)
            {
                return(true);
            }
            RegIntZone_READY.Release(robot);
            RegIntZone_GATE12.Release(robot);
            return(false);
        }
        public static bool Reg_checkinC2_Ready(RobotUnity robot, TrafficManagementService traffic)
        {
            bool  onRegG12   = false;
            bool  onRegReady = false;
            Point Rloc       = robot.properties.pose.Position;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("READY-GATE", robot))
            {
                return(false);
            }
            if (RegIntZone_GATE12.ProcessRegistryIntersectionZone(robot))
            {
                onRegG12 = true;
            }

            if (RegIntZone_READY.ProcessRegistryIntersectionZone(robot))
            {
                onRegReady = true;
            }

            if (onRegG12 && onRegReady)
            {
                return(true);
            }
            RegIntZone_GATE12.Release(robot);
            RegIntZone_READY.Release(robot);
            return(false);
        }
        // Từ C1 -> GATE
        // Detect no Robot : Ready , G12
        // Check register Elevator -> Ready ->Gate 12
        public static bool Reg_checkinC1_GATE(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegReady = false;
            bool onRegGate  = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            //if (traffic.HasOtherRobotUnityinArea("READY", robot) || traffic.HasOtherRobotUnityinArea("GATE", robot))
            if (traffic.HasOtherRobotUnityinArea("READY", robot) || traffic.HasOtherRobotUnityinArea("GATE", robot))
            {
                return(false);
            }
            if (RegIntZone_READY.ProcessRegistryIntersectionZone(robot))
            {
                onRegReady = true;
            }
            if (RegIntZone_GATEZONE.ProcessRegistryIntersectionZone(robot))
            {
                onRegGate = true;
            }
            if (onRegReady && onRegGate)
            {
                return(true);
            }
            RegIntZone_READY.Release(robot);
            RegIntZone_GATEZONE.Release(robot);
            return(false);
        }
 public void Initialize()
 {
     RobotManagementServiceRegister     = new RobotManagementService();
     DoorManagementServiceRegister      = new DoorManagementService();
     ProcedureManagementServiceRegister = new ProcedureManagementService();
     TrafficManagementServiceRegister   = new TrafficManagementService();
 }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureMachineToReturn(RobotUnity robot, TrafficManagementService traffiicService) : base(robot, traffiicService)
 {
     StateMachineToReturn = MachineToReturn.MACRET_IDLE;
     this.robot           = robot;
     this.Traffic         = traffiicService;
     procedureCode        = ProcedureCode.PROC_CODE_MACHINE_TO_RETURN;
 }
        // Ready -> ELevator, VIM (C5)
        public static bool Reg_checkinReady__ElevatorAndVIM(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegG12  = false;
            bool onRegElev = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("GATE12", robot) || traffic.HasOtherRobotUnityinArea("ELEVATOR", robot))
            {
                return(false);
            }
            if (RegIntZone_GATE12.ProcessRegistryIntersectionZone(robot))
            {
                onRegG12 = true;
            }
            if (RegIntZone_ELEVATOR.ProcessRegistryIntersectionZone(robot))
            {
                onRegElev = true;
            }
            if (onRegG12 && onRegElev)
            {
                return(true);
            }
            RegIntZone_GATE12.Release(robot);
            RegIntZone_ELEVATOR.Release(robot);
            return(false);
        }
        // Từ C2 -> Gate 3
        // Detect no Robot : G12, Elevator ,G3
        // Check register G3 -> Ready ->Gate 12 -> Elevator
        public static bool Reg_checkinC2_G3(RobotUnity robot, TrafficManagementService traffic)
        {
            bool  onRegG12  = false;
            bool  onRegElev = false;
            bool  onRegG3   = false;
            Point Rloc      = robot.properties.pose.Position;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("GATE12", robot) || traffic.HasOtherRobotUnityinArea("ELEVATOR", robot) || traffic.HasOtherRobotUnityinArea("GATE3", robot))
            {
                return(false);
            }
            if (RegIntZone_GATE12.ProcessRegistryIntersectionZone(robot))
            {
                onRegG12 = true;
            }
            if (RegIntZone_ELEVATOR.ProcessRegistryIntersectionZone(robot))
            {
                onRegElev = true;
            }
            if (RegIntZone_GATE3.ProcessRegistryIntersectionZone(robot))
            {
                onRegG3 = true;
            }
            if (onRegG12 && onRegElev && onRegG3)
            {
                return(true);
            }
            RegIntZone_GATE12.Release(robot);
            RegIntZone_ELEVATOR.Release(robot);
            RegIntZone_GATE3.Release(robot);
            return(false);
        }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureBufferReturnToBuffer401(RobotUnity robot, TrafficManagementService trafficService) : base(robot, trafficService)
 {
     StateBufferToBuffer = BufferToBuffer.BUFTOBUF_IDLE;
     this.robot          = robot;
     // this.points = new DataBufferToBuffer();
     this.Traffic  = trafficService;
     procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_BUFFER;
 }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureBufferToMachine(RobotUnity robot, TrafficManagementService trafficService) : base(robot, trafficService)
 {
     StateBufferToMachine = BufferToMachine.BUFMAC_IDLE;
     this.robot           = robot;
     // this.points = new DataBufferToMachine();
     this.Traffic  = trafficService;
     procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE;
 }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureBufferToReturn(RobotUnity robot, TrafficManagementService trafficService) : base(robot)
 {
     StateBufferToReturn = BufferToReturn.BUFRET_IDLE;
     this.robot          = robot;
     // this.points = new DataForkBufferToReturn();
     this.Traffic  = trafficService;
     procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_RETURN;
 }
예제 #11
0
 public void Registry(TrafficManagementService trafficManagementService)
 {
     this.trafficManagementService = trafficManagementService;
     foreach (RobotUnity robot in RobotUnityRegistedList.Values)
     {
         robot.Registry(this.trafficManagementService);
     }
 }
        public void Initialize()
        {
            robotManagementService = new RobotManagementService(this.mainWindow.map);
            doorManagementService  = new DoorManagementService();
            DataReceive status = new DataReceive();

            // Test door
            ///   doorManagementService.DoorMezzamineUpNew.LampSetStateOn(DoorType.DOOR_FRONT);
            //while (true)
            //{
            //    doorManagementService.DoorMezzamineUpNew.LampOn(DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.LampOff(DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.OpenPress(DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.GetStatus(ref status, DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.ClosePress(DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.GetStatus(ref status, DoorType.DOOR_FRONT);
            //    doorManagementService.DoorMezzamineUpNew.OpenPress(DoorType.DOOR_BACK);
            //    doorManagementService.DoorMezzamineUpNew.GetStatus(ref status, DoorType.DOOR_BACK);
            //    doorManagementService.DoorMezzamineUpNew.ClosePress(DoorType.DOOR_BACK);
            //    doorManagementService.DoorMezzamineUpNew.GetStatus(ref status, DoorType.DOOR_BACK);

            //}
            // End

            procedureManagementService = new ProcedureManagementService();
            chargerService             = new ChargerManagementService();
            trafficService             = new TrafficManagementService();
            deviceRegistrationService  = new DeviceRegistrationService(12000);
            Global_Object.doorManagementServiceCtrl = doorManagementService;
            assigmentTaskService = new AssigmentTaskService();
            assigmentTaskService.RegistryService(robotManagementService);
            assigmentTaskService.RegistryService(procedureManagementService);
            assigmentTaskService.RegistryService(deviceRegistrationService.GetDeviceItemList());
            assigmentTaskService.RegistryService(trafficService);
            procedureManagementService.RegistryService(trafficService);
            procedureManagementService.RegistryService(robotManagementService);
            procedureManagementService.RegistryService(doorManagementService);
            procedureManagementService.RegistryService(chargerService);
            procedureManagementService.RegistryService(deviceRegistrationService);
            procedureManagementService.RegistryService(assigmentTaskService);


            robotManagementService.Registry(trafficService);
            // robotManagementService.TestRobotProceure();
            robotManagementService.Initialize();
            //robotManagementService.robot2test();

            deviceRegistrationService.listen();
            deviceRegistrationService.RegistryMainWindow(this.mainWindow);

            //assigmentTaskService.Start();
            MessageBox.Show("Bấm Start Để Bắt Đầu !");
            RobotMoving robotMoving = new RobotMoving(robotManagementService.RobotUnityRegistedList);

            robotMoving.Show();

            //assigmentTaskService.Start();
        }
 public ProcedureForkLiftToBuffer(RobotUnity robot, DoorManagementService doorservice, TrafficManagementService trafficService) : base(robot, doorservice, trafficService)
 {
     StateForkLift    = ForkLift.FORBUF_IDLE;
     resCmd           = ResponseCommand.RESPONSE_NONE;
     this.robot       = robot;
     this.doorservice = doorservice;
     this.Traffic     = trafficService;
     procedureCode    = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER;
 }
예제 #14
0
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureRobotToReady(RobotUnity robot, ChargerId id, TrafficManagementService trafficService, ChargerManagementService chargerService, Pose PointCheckIn) : base(robot, trafficService)
 {
     StateRobotGoToReady = RobotGoToReady.ROBREA_IDLE;
     this.robot          = robot;
     this.Traffic        = trafficService;
     this.charger        = chargerService;
     this.chardgeId      = id;
     this.robot.ShowText("this.chardgeId" + this.chardgeId);
     procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY;
 }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureForkLiftToMachine(RobotUnity robot, DoorManagementService doorservice, TrafficManagementService traffiicService) : base(robot)
 {
     StateForkLiftToMachine = ForkLiftToMachine.FORMACH_IDLE;
     resCmd           = ResponseCommand.RESPONSE_NONE;
     this.robot       = robot;
     this.doorservice = doorservice;
     //  door = doorservice.DoorMezzamineUp;
     this.Traffic  = traffiicService;
     procedureCode = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER;
 }
 // public override event Action<Object> ErrorProcedureHandler;
 public ProcedureReturnToGate(RobotUnity robot, DoorManagementService doorservice, TrafficManagementService traffiicService) : base(robot)
 {
     StateReturnToGate = ReturnToGate.RETGATE_IDLE;
     resCmd            = ResponseCommand.RESPONSE_NONE;
     this.robot        = robot;
     base.robot        = robot;
     // this.points = new DataReturnToGate();
     this.door     = doorservice;
     this.Traffic  = traffiicService;
     errorCode     = ErrorCode.RUN_OK;
     procedureCode = ProcedureCode.PROC_CODE_RETURN_TO_GATE;
 }
        // Từ OUTER -> MERZE
        // Detect no Robot : Ready
        // Check register Ready
        public static bool Reg_checkinMerz(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegMerz = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("MERZ", robot))
            {
                return(false);
            }
            if (RegIntZone_MERZ.ProcessRegistryIntersectionZone(robot))
            {
                onRegMerz = true;
            }
            if (onRegMerz)
            {
                return(true);
            }
            RegIntZone_MERZ.Release(robot);
            return(false);
        }
        // Từ Elevator(C5) -> Gate3
        // Detect no Robot : GATE3
        public static bool Reg_checkinElevator_Gate3(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegG3 = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("GATE3", robot))
            {
                return(false);
            }
            if (RegIntZone_GATE3.ProcessRegistryIntersectionZone(robot))
            {
                onRegG3 = true;
            }
            if (onRegG3)
            {
                return(true);
            }
            RegIntZone_GATE3.Release(robot);
            return(false);
        }
        // Từ Gate12 ( C3) -> VIM and ELEVATOR
        // Detect no Robot : Elevator
        public static bool Reg_checkinGate12_ElevatorAndVIM(RobotUnity robot, TrafficManagementService traffic)
        {
            bool  onRegElev = false;
            Point Rloc      = robot.properties.pose.Position;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("ELEVATOR", robot))
            {
                return(false);
            }
            if (RegIntZone_ELEVATOR.ProcessRegistryIntersectionZone(robot))
            {
                onRegElev = true;
            }
            if (onRegElev)
            {
                return(true);
            }
            RegIntZone_ELEVATOR.Release(robot);
            return(false);
        }
        // Từ C2 -> Gate12
        // Detect no Robot : G12
        // Check register Gate 12
        public static bool Reg_checkinC2_G12(RobotUnity robot, TrafficManagementService traffic)
        {
            bool onRegG12 = false;

            //kiem tra có robot trong vùng này, nếu có trả về false
            if (traffic.HasOtherRobotUnityinArea("GATE12", robot) ||
                traffic.HasOtherRobotUnityinArea("ELEVATOR", robot) ||
                traffic.HasOtherRobotUnityinArea("GATE3", robot) ||
                traffic.HasOtherRobotUnityinArea("READY-GATE", robot) ||
                traffic.HasOtherRobotUnityinArea("C1", robot))
            {
                return(false);
            }
            if (RegIntZone_GATE12.ProcessRegistryIntersectionZone(robot))
            {
                onRegG12 = true;
            }
            if (onRegG12)
            {
                return(true);
            }
            RegIntZone_GATE12.Release(robot);
            return(false);
        }
        public void Procedure(object ojb)
        {
            ProcedureReturnToGate ReToGate = (ProcedureReturnToGate)ojb;
            RobotUnity            rb       = ReToGate.robot;

            // DataReturnToGate p = ReToGate.points;
            ds = ReToGate.door.DoorMezzamineReturn;
            ds.setRb(rb);
            TrafficManagementService Traffic = ReToGate.Traffic;

            rb.mcuCtrl.lampRbOn();
            robot.ShowText(" Start -> " + procedureCode);
            while (ProRun)
            {
                switch (StateReturnToGate)
                {
                case ReturnToGate.RETGATE_IDLE:
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_CHECKIN_RETURN:     // doi robot di den khu vuc checkin cua vung buffer
                    try
                    {
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
                            {
                                Stopwatch sw = new Stopwatch();
                                sw.Start();
                                do
                                {
                                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                                    {
                                        if (rb.SendPoseStamped(ReToGate.GetCheckInBuffer()))
                                        {
                                            resCmd            = ResponseCommand.RESPONSE_NONE;
                                            StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_ZONE_RETURN_READY;
                                            robot.ShowText("RETGATE_ROBOT_WAITTING_ZONE_RETURN_READY");
                                            break;
                                        }
                                    }
                                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                                    {
                                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                                        CheckUserHandleError(this);
                                        break;
                                    }
                                    if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                                    {
                                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                                        CheckUserHandleError(this);
                                        break;
                                    }
                                    Thread.Sleep(100);
                                } while (ProRunStopW);
                                sw.Stop();
                            }
                        }
                        else
                        {
                            if (rb.SendPoseStamped(ReToGate.GetCheckInBuffer()))
                            {
                                StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_ZONE_RETURN_READY;
                                robot.ShowText("RETGATE_ROBOT_WAITTING_ZONE_RETURN_READY");
                            }
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_ZONE_RETURN_READY:     // doi khu vuc buffer san sang de di vao
                    try
                    {
                        if (false == robot.CheckInZoneBehavior(ReToGate.GetFrontLineReturn().Position))
                        {
                            if (rb.SendPoseStamped(ReToGate.GetFrontLineReturn()))
                            {
                                StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_CAME_FRONTLINE_RETURN;
                                robot.ShowText("RETGATE_ROBOT_WAITTING_CAME_FRONTLINE_RETURN");
                            }
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_CAME_FRONTLINE_RETURN:
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            resCmd            = ResponseCommand.RESPONSE_NONE;
                            StateReturnToGate = ReturnToGate.RETGATE_ROBOT_SEND_CMD_PICKUP_PALLET;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_SEND_CMD_PICKUP_PALLET:
                    if (rb.SendCmdAreaPallet(ReToGate.GetInfoOfPalletReturn(PistonPalletCtrl.PISTON_PALLET_UP)))
                    {
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_PICKUP_PALLET_RETURN;
                        robot.ShowText("RETGATE_ROBOT_WAITTING_PICKUP_PALLET_RETURN");
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_PICKUP_PALLET_RETURN:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        ReToGate.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId);
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_RETURN;
                        robot.ShowText("RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_RETURN");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_RETURN:     // đợi
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        if (rb.SendPoseStamped(ds.config.PointCheckInGate))
                        {
                            resCmd            = ResponseCommand.RESPONSE_NONE;
                            StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_CHECKIN_GATE;
                            robot.ShowText("RETGATE_ROBOT_WAITTING_GOTO_CHECKIN_GATE");
                        }
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                // case ReturnToGate.RETGATE_ROBOT_GOTO_CHECKIN_GATE: //gui toa do di den khu vuc checkin cong
                //     rb.SendPoseStamped(ds.config.PointCheckInGate);
                //     StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_CHECKIN_GATE;
                //     break;
                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_CHECKIN_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_CAME_CHECKIN_GATE;
                        robot.ShowText("RETGATE_ROBOT_CAME_CHECKIN_GATE");
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_CAME_CHECKIN_GATE:     // đã đến vị trí, kiem tra va cho khu vuc cong san sang de di vao.
                    if (false == robot.CheckInZoneBehavior(ds.config.PointFrontLine.Position))
                    {
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        if (rb.SendPoseStamped(ds.config.PointFrontLine))
                        {
                            StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_GATE;
                            robot.ShowText("RETGATE_ROBOT_WAITTING_GOTO_GATE");
                        }
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_GATE:
                    if (Traffic.RobotIsInArea("GATE3", rb.properties.pose.Position))
                    {
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_GATE_OPENDOOR;
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOTO_GATE_OPENDOOR:
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_CAME_GATE_POSITION;
                        robot.ShowText("RETGATE_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_CAME_GATE_POSITION:     // da den khu vuc cong , gui yeu cau mo cong.
                    //ds.setDoorBusy(true);
                    //ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_OPEN_DOOR;
                    robot.ShowText("RETGATE_ROBOT_WAITTING_OPEN_DOOR");
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_OPEN_DOOR:     //doi mo cong
                    RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK);
                    if (ret == RetState.DOOR_CTRL_SUCCESS)
                    {
                        if (rb.SendCmdAreaPallet(ds.config.infoPallet))
                        {
                            StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_DROPDOWN_PALLET_RETURN;
                            robot.ShowText("RETGATE_ROBOT_WAITTING_DROPDOWN_PALLET_RETURN");
                        }
                    }
                    else if (ret == RetState.DOOR_CTRL_ERROR)
                    {
                        robot.ShowText("RETGATE_ROBOT_WAITTING_OPEN_DOOR_ERROR__(-_-)");
                        Thread.Sleep(1000);
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    }
                    break;

                // case ReturnToGate.RETGATE_ROBOT_OPEN_DOOR_SUCCESS: // mo cua thang cong ,gui toa do line de robot di vao
                //     rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETDOWN);
                //     StateReturnToGate = ReturnToGate.RETGATE_ROBOT_GOTO_POSITION_PALLET_RETURN;
                //     break;
                // case ReturnToGate.RETGATE_ROBOT_GOTO_POSITION_PALLET_RETURN:
                //     if (true == rb.CheckPointDetectLine(ds.config.PointOfPallet, rb))
                //     {
                //         rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_LINEDETECT_COMING_POSITION);
                //         StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_DROPDOWN_PALLET_RETURN;
                //     }
                //     break;
                case ReturnToGate.RETGATE_ROBOT_WAITTING_DROPDOWN_PALLET_RETURN:     // doi robot gap hang
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        // ReToGate.UpdatePalletState(PalletStatus.W);
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
                        robot.ShowText("RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        ds.closeDoor(DoorService.DoorType.DOOR_BACK);
                        ds.setDoorBusy(false);
                        StateReturnToGate = ReturnToGate.RETGATE_ROBOT_WAITTING_CLOSE_GATE;
                        robot.ShowText("RETGATE_ROBOT_WAITTING_CLOSE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ReturnToGate.RETGATE_ROBOT_WAITTING_CLOSE_GATE:     // doi dong cong.
                    //if (true == ds.WaitClose(DoorService.DoorType.DOOR_BACK, TIME_OUT_CLOSE_DOOR))
                    //{
                    StateReturnToGate = ReturnToGate.RETGATE_ROBOT_RELEASED;
                    //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                    robot.ShowText("RETGATE_ROBOT_WAITTING_CLOSE_GATE");
                    //}
                    //else
                    //{
                    //    errorCode = ErrorCode.CLOSE_DOOR_ERROR;
                    //    CheckUserHandleError(this);
                    //}
                    break;

                case ReturnToGate.RETGATE_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    ds.removeListCtrlDoorBack();
                    robot.robotTag    = RobotStatus.IDLE;
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_RETURN_TO_GATE;
                    // if (errorCode == ErrorCode.RUN_OK) {
                    ReleaseProcedureHandler(this);
                    // } else {
                    // ErrorProcedureHandler (this);
                    // }
                    ProRun = false;
                    robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                default:
                    break;
                }
                Thread.Sleep(5);
            }
            StateReturnToGate = ReturnToGate.RETGATE_IDLE;
        }
        public void Procedure(object ojb)
        {
            ProcedureBufferReturnToBuffer401 BfToBf = (ProcedureBufferReturnToBuffer401)ojb;
            RobotUnity rb = BfToBf.robot;
            TrafficManagementService Traffic = BfToBf.Traffic;

            robot.ShowText(" Start -> " + procedureCode);
            rb.mcuCtrl.lampRbOn();
            while (ProRun)
            {
                switch (StateBufferToBuffer)
                {
                case BufferToBuffer.BUFTOBUF_IDLE:
                    //robot.ShowText("BUFTOBUF_IDLE");
                    break;

                case BufferToBuffer.BUFTOBUF_SELECT_BEHAVIOR_ONZONE_BUFFER_A:
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
                    {
                        Pose endPose = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn);
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_GOTO_BACK_FRONTLINE_READY;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = endPose.Position;
                        }
                    }
                    else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position))
                    {
                        Pose endPose = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn);
                        if (rb.SendPoseStamped(endPose))
                        {
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = endPose.Position;
                            //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                        }
                    }
                    else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position))
                    {
                        Pose   destPos1  = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn);
                        String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE);
                        if (destName1.Equals("OUTER"))
                        {
                            //robot.ShowText("GO FRONTLINE IN OUTER");
                            if (rb.SendPoseStamped(destPos1))
                            {
                                StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                        else if (destName1.Equals("VIM"))
                        {
                            //robot.ShowText("GO FRONTLINE IN VIM");
                            if (rb.SendPoseStamped(destPos1))
                            {
                                StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                //robot.ShowText("CHECK - REG");
                            }
                        }
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Start();
                        do
                        {
                            robot.onFlagGoBackReady = true;
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                            {
                                robot.onFlagGoBackReady = false;

                                Pose   destPos2  = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn);
                                String destName2 = Traffic.DetermineArea(destPos2.Position, TypeZone.MAIN_ZONE);
                                if (destName2.Equals("OUTER"))
                                {
                                    //robot.ShowText("GO FRONTLINE IN OUTER");
                                    if (rb.SendPoseStamped(destPos2))
                                    {
                                        resCmd = ResponseCommand.RESPONSE_NONE;
                                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos2.Position;
                                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                    }
                                }
                                else if (destName2.Equals("VIM"))
                                {
                                    //robot.ShowText("GO FRONTLINE IN VIM");
                                    if (rb.SendPoseStamped(destPos2))
                                    {
                                        resCmd = ResponseCommand.RESPONSE_NONE;
                                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_READY;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos2.Position;
                                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                        //robot.ShowText("CHECK - REG");
                                    }
                                }
                                break;
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            Thread.Sleep(100);
                        } while (ProRunStopW);
                        sw.Stop();
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_READY:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                    {
                        break;
                    }

                    try
                    {
                        //bool onComePoint2 = robot.ReachedGoal();
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (onComePoint2)
                        {
                            //

                            robot.SwitchToDetectLine(true);

                            resCmd = ResponseCommand.RESPONSE_NONE;
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG:

                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM;
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                    {
                        break;
                    }
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);

                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A:
                    try
                    {
                        //bool onComePoint2 = robot.ReachedGoal();
                        if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                        {
                            break;
                        }
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (onComePoint2)
                        {
                            robot.SwitchToDetectLine(true);
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A:
                    if (rb.SendCmdAreaPallet(BfToBf.GetInfoOfPalletBufferReturn_BRB401(PistonPalletCtrl.PISTON_PALLET_UP, order.dataRequest_BufferReturn)))
                    {
                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_PICKUP_PALLET_BUFFER_A;
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_PICKUP_PALLET_BUFFER_A:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        BfToBf.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId);
                        onUpdatedPalletState = true;
                        StateBufferToBuffer  = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A;
                        //robot.ShowText("BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A:     // đợi
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                        {
                            robot.bayId    = -1;
                            robot.bayIdReg = false;
                            robot.ReleaseWorkingZone();
                            robot.SwitchToDetectLine(false);

                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_BUFFER_B_SELECT_ZONE;
                            // cập nhật lại điểm xuất phát
                            registryRobotJourney.startPoint = robot.properties.pose.Position;
                            //robot.ShowText("BUFTOBUF_ROBOT_WAITTING_GOTO_FRONTLINE_BUFFER_B");
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_BUFFER_B_SELECT_ZONE:
                    String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
                    Pose   destPos        = BfToBf.GetFrontLineBuffer401_BRB401(order.dataRequest_Buffer401);
                    String destName       = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
                    if (startNamePoint.Equals("VIM"))
                    {
                        if (rb.SendPoseStamped(destPos))
                        {
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = destPos.Position;
                            //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                        }
                    }
                    else if (startNamePoint.Equals("OUTER"))
                    {
                        if (destName.Equals("OUTER"))
                        {
                            if (rb.SendPoseStamped(destPos))
                            {
                                StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                        else if (destName.Equals("VIM"))
                        {
                            if (rb.SendPoseStamped(destPos))
                            {
                                StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                    }

                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG:
                    if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                    {
                        break;
                    }
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM;
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM:
                    try
                    {
                        TrafficRountineConstants.DetectRelease(registryRobotJourney);
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B:
                    try
                    {
                        if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                        {
                            break;
                        }
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);

                            resCmd = ResponseCommand.RESPONSE_NONE;
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM:
                    if (rb.SendCmdAreaPallet(BfToBf.GetInfoOfPalletBuffer401_BRB401(PistonPalletCtrl.PISTON_PALLET_DOWN, order.dataRequest_Buffer401)))
                    {
                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B;
                        robot.ShowText("BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B");
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        BfToBf.UpdatePalletState(PalletStatus.W);
                        //onUpdatedPalletState = true;
                        StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B;
                        //robot.ShowText("FBUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B:     // đợi
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                        {
                            robot.bayId    = -1;
                            robot.bayIdReg = false;
                            robot.ReleaseWorkingZone();
                            robot.SwitchToDetectLine(false);

                            resCmd = ResponseCommand.RESPONSE_NONE;

                            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_RELEASED;
                            //robot.ShowText("BUFTOBUF_ROBOT_RELEASED");
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    robot.bayId = -1;
                    TrafficRountineConstants.ReleaseAll(robot);
                    robot.orderItem = null;
                    robot.SwitchToDetectLine(false);
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_BUFFER;
                    ReleaseProcedureHandler(this);
                    ProRun = false;
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.status             = StatusOrderResponseCode.FINISHED;
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                case BufferToBuffer.BUFTOBUF_ROBOT_DESTROY:
                    TrafficRountineConstants.ReleaseAll(robot);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    robot.SwitchToDetectLine(false);
                    robot.ReleaseWorkingZone();
                    // StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_RELEASED;
                    //robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
                    ProRun = false;
                    UpdateInformationInProc(this, ProcessStatus.F);
                    order.status = StatusOrderResponseCode.ROBOT_ERROR;
                    //reset status pallet Faile H->Ws
                    if (!onUpdatedPalletState)
                    {
                        BfToBf.UpdatePalletState(PalletStatus.F, order.palletId_P, order.planId);
                    }
                    selectHandleError = SelectHandleError.CASE_ERROR_EXIT;
                    procedureStatus   = ProcedureStatus.PROC_KILLED;
                    FreeHoldBuffer(order.palletId_H);
                    KillEvent();

                    //this.robot.DestroyRegistrySolvedForm();
                    break;

                default:
                    break;
                }
                Thread.Sleep(5);
            }
            StateBufferToBuffer = BufferToBuffer.BUFTOBUF_IDLE;
        }
예제 #23
0
        public void Procedure(object ojb)
        {
            ProcedureRobotToReady    RbToRd  = (ProcedureRobotToReady)ojb;
            RobotUnity               rb      = RbToRd.robot;
            DataRobotToReady         p       = RbToRd.points;
            TrafficManagementService Traffic = RbToRd.Traffic;

            robot.ShowText(" start -> " + procedureCode + "_________________0000000000000000000_____________________");
            while (ProRun)
            {
                switch (StateRobotGoToReady)
                {
                case RobotGoToReady.ROBREA_IDLE:
                    //robot.ShowText("ROBREA_IDLE");
                    break;

                case RobotGoToReady.ROBREA_SELECT_BEHAVIOR_ONZONE:
                    robot.ShowText("ROBREA_SELECT_BEHAVIOR_ONZONE");
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS))
                    {
                        robot.ShowText("ROBREA_ROBOT_RELEASED");
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_RELEASED;
                    }
                    if (rb.SendPoseStamped(p.PointFrontLine))
                    {
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_WAITTING_GOTO_READYSTATION_REG;
                        robot.ShowText("ROBREA_ROBOT_GOTO_FRONTLINE_READYSTATION");
                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                        registryRobotJourney.endPoint       = p.PointFrontLine.Position;
                    }
                    break;

                case RobotGoToReady.ROBREA_ROBOT_WAITTING_GOTO_READYSTATION_REG:     // Robot dang di toi dau line ready station
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        if (DetermineHasTaskWaitingAnRobotAvailable())
                        {
                            StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_WAITINGREADY_FORCERELEASED;
                            robot.ShowText("ROBREA_ROBOT_WAITINGREADY_FORCERELEASED");
                            break;
                        }
                    }
                    else
                    {
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_WAITTING_GOTO_READYSTATION;
                    }
                    break;

                case RobotGoToReady.ROBREA_ROBOT_WAITTING_GOTO_READYSTATION:     // Robot dang di toi dau line ready station

                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        TrafficRountineConstants.DetectRelease(registryRobotJourney);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_WAIITNG_DETECTLINE_TO_READYSTATION;
                        robot.ShowText("ROBREA_ROBOT_WAIITNG_DETECTLINE_TO_READYSTATION");
                    }
                    break;

                case RobotGoToReady.ROBREA_ROBOT_WAIITNG_DETECTLINE_TO_READYSTATION:     // đang đợi dò line để đến vị trí line trong buffer
                    if (rb.SendCmdAreaPallet(RbToRd.points.PointOfCharger))
                    {
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_WAITTING_CAME_POSITION_READYSTATION;
                        robot.ShowText("ROBREA_ROBOT_WAITTING_CAME_POSITION_READYSTATION");
                    }
                    break;

                case RobotGoToReady.ROBREA_ROBOT_WAITTING_CAME_POSITION_READYSTATION:     // đến vị trả robot về robotmanagement để nhận quy trình mới
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOTO_POSITION)
                    {
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateRobotGoToReady = RobotGoToReady.ROBREA_ROBOT_RELEASED;
                        TrafficRountineConstants.RegIntZone_READY.Release(robot);
                        robot.robotTag = RobotStatus.IDLE;
                        robot.SetSafeYellowcircle(false);
                        robot.SetSafeBluecircle(false);
                        robot.SetSafeSmallcircle(false);
                        robot.TurnOnSupervisorTraffic(false);
                        rb.mcuCtrl.lampRbOff();
                        robot.ShowText("ROBREA_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case RobotGoToReady.ROBREA_ROBOT_RELEASED:
                    Global_Object.cntGoready++;
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_READY;
                    // if (errorCode == ErrorCode.RUN_OK) {
                    ReleaseProcedureHandler(this);
                    // } else {
                    //     ErrorProcedureHandler (this);
                    // }
                    robot.setColorRobotStatus(RobotStatusColorCode.ROBOT_STATUS_OK);
                    ProRun = false;
                    //robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                case RobotGoToReady.ROBREA_ROBOT_WAITINGREADY_FORCERELEASED:
                    // add to wait task;
                    //robot.robotTag = RobotStatus.IDLE;

                    robot.setColorRobotStatus(RobotStatusColorCode.ROBOT_STATUS_OK);
                    procedureCode = ProcedureControlServices.ProcedureCode.PROC_CODE_ROBOT_WAITINGTO_READY;
                    ProRun        = false;
                    robot.ShowText("RELEASED WHEN WAITTING TO READY, HAS AN NEW TASK");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;

                    rb.PreProcedureAs = ProcedureControlAssign.PRO_WAIT_TASK;
                    ReleaseProcedureHandler(this);
                    KillEvent();
                    break;
                }
                Thread.Sleep(700);
            }
            StateRobotGoToReady = RobotGoToReady.ROBREA_IDLE;
        }
        public void Procedure(object ojb)
        {
            ProcedureForkLiftToBuffer FlToBuf = (ProcedureForkLiftToBuffer)ojb;
            RobotUnity rb = FlToBuf.robot;

            doorServiceCtrl = new DoorServiceCtrl();
            doorServiceCtrl = getDoorService();

            ds = doorServiceCtrl.doorService;
            ds.setRb(rb);
            TrafficManagementService Traffic         = FlToBuf.Traffic;
            ForkLiftToMachineInfo    flToMachineInfo = new ForkLiftToMachineInfo();

            rb.mcuCtrl.lampRbOn();
            robot.ShowText(" Start -> " + procedureCode);

            /*  endPointBuffer = FlToBuf.GetFrontLineBuffer(true);
             * if (endPointBuffer == null)
             * {
             *    robot.bayId = -1;
             *    Console.WriteLine("Error Data Request" + order.dataRequest);
             *    order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
             *    TrafficRountineConstants.ReleaseAll(robot);
             *    robot.orderItem = null;
             *    robot.SwitchToDetectLine(false);
             *    robot.ReleaseWorkingZone();
             *    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
             *    {
             *        TrafficRountineConstants.RegIntZone_READY.Release(robot);
             *        robot.robotTag = RobotStatus.IDLE;
             *        robot.SetSafeYellowcircle(false);
             *        robot.SetSafeBluecircle(false);
             *        robot.SetSafeSmallcircle(false);
             *        robot.TurnOnSupervisorTraffic(false);
             *       // rb.mcuCtrl.lampRbOff();
             *        procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY;
             *    }
             *    else
             *        procedureCode = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER;
             *    ReleaseProcedureHandler(this);
             *    ProRun = false;
             *    robot.ShowText("RELEASED");
             *    UpdateInformationInProc(this, ProcessStatus.S);
             *    order.endTimeProcedure = DateTime.Now;
             *    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
             *    SaveOrderItem(order);
             *    KillEvent();
             * }
             * else
             * {
             *    order.frontLinePos = endPointBuffer.Position;
             *    robot.bayId = bayId;
             * }*/
            while (ProRun)
            {
                // JPallet aa= GetInfoPallet_P_InBuffer(TrafficRobotUnity.PistonPalletCtrl.PISTON_PALLET_DOWN);
                switch (StateForkLift)
                {
                case ForkLift.FORBUF_IDLE:
                    robot.ShowText("FORBUF_IDLE");
                    break;

                case ForkLift.FORBUF_GET_FRONTLINE:
                    endPointBuffer = FlToBuf.GetFrontLineBuffer(true);
                    if (endPointBuffer == null)
                    {
                        if (countFrontLineNull++ < 10)
                        {
                            break;
                        }
                        countFrontLineNull = 0;
                        robot.bayId        = -1;
                        robot.ShowText("Error Data Request FrontLine ForkLift" + order.dataRequest);
                        order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
                        TrafficRountineConstants.ReleaseAll(robot);
                        robot.orderItem = null;
                        robot.SwitchToDetectLine(false);
                        robot.ReleaseWorkingZone();
                        if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
                        {
                            TrafficRountineConstants.RegIntZone_READY.Release(robot);
                            robot.robotTag = RobotStatus.IDLE;
                            robot.SetSafeYellowcircle(false);
                            robot.SetSafeBluecircle(false);
                            robot.SetSafeSmallcircle(false);
                            robot.TurnOnSupervisorTraffic(false);
                            // rb.mcuCtrl.lampRbOff();
                            procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY;
                        }
                        else
                        {
                            procedureCode = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER;
                        }

                        ProRun = false;
                        robot.ShowText("RELEASED FL---");
                        UpdateInformationInProc(this, ProcessStatus.S);
                        order.endTimeProcedure   = DateTime.Now;
                        order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                        SaveOrderItem(order);
                        KillEvent();
                        ReleaseProcedureHandler(this);
                    }
                    else
                    {
                        order.frontLinePos = endPointBuffer.Position;
                        robot.bayId        = bayId;
                        StateForkLift      = ForkLift.FORBUF_SELECT_BEHAVIOR_ONZONE;
                    }
                    break;

                case ForkLift.FORBUF_SELECT_BEHAVIOR_ONZONE:
                    order.status = StatusOrderResponseCode.GOING_AND_PICKING_UP;
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
                    {
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            //if (false == rb.CheckInGateFromReadyZoneBehavior(doorServiceCtrl.PointFrontLine.Position))
                            {
                                robot.ShowText("FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY");
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = doorServiceCtrl.PointFrontLine.Position;
                                StateForkLift = ForkLift.FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY;
                            }
                        }
                    }
                    else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position))
                    {
                        robot.robotTag = RobotStatus.WORKING;
                        if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine))
                        {
                            StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG;
                            // Cap Nhat Thong Tin CHuyen Di
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = doorServiceCtrl.PointFrontLine.Position;
                            robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE");
                        }
                    }
                    else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position))
                    {
                        // public void Start (ForkLiftToBuffer state = ForkLiftToBuffer.FORBUF_ROBOT_RELEASED) {
                        robot.robotTag = RobotStatus.WORKING;
                        if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine))
                        {
                            StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = doorServiceCtrl.PointFrontLine.Position;
                            robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_CHECKIN_GATE");
                        }
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    robot.ShowText("Detetect Inside Station Check Ready");
                    if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_LEFT))
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Start();
                        do
                        {
                            robot.onFlagGoBackReady = true;
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE || Traffic.RobotIsInArea("READY_FRONTLINE", robot.properties.pose.Position))
                            {
                                robot.onFlagGoBackReady = false;
                                robot.robotTag          = RobotStatus.WORKING;
                                if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine))
                                {
                                    resCmd        = ResponseCommand.RESPONSE_NONE;
                                    StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY;
                                    robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE");
                                    break;
                                }
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            Thread.Sleep(100);
                        } while (ProRunStopW);
                        sw.Stop();
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY:
                    // dò ra điểm đích đến và xóa đăng ký vùng
                    // TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (Traffic.RobotIsInArea("C3", rb.properties.pose.Position))
                    {
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY_OPEN_DOOR;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY_OPEN_DOOR:
                    // dò ra điểm đích đến và xóa đăng ký vùng
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        robot.setTrafficAllCircles(false, false, false, false);
                        TrafficRountineConstants.RegIntZone_READY.Release(robot);
                        robot.SwitchToDetectLine(true);
                        resCmd        = ResponseCommand.RESPONSE_NONE;
                        StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION;
                        robot.ShowText("FORBUF_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG:
                    // kiem tra vung đăng ký tai khu vuc xac định
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        // dò ra điểm đích đến và xóa đăng ký vùng
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM:
                    // kiem tra vung đăng ký tai khu vuc xac định
                    //TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (Traffic.RobotIsInArea("C3", rb.properties.pose.Position))
                    {
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR:
                    // kiem tra vung đăng ký tai khu vuc xac định
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        // robot.setTrafficAllCircles(false, false, false, false);
                        TrafficRountineConstants.RegIntZone_READY.Release(robot);
                        robot.SwitchToDetectLine(true);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION;
                        robot.ShowText("FORBUF_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION:     // da den khu vuc cong , gui yeu cau mo cong.
                    robot.robotRegistryToWorkingZone.onRobotwillCheckInsideGate = false;
                    //ds.setDoorBusy(true);
                    //ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_OPEN_DOOR;
                    robot.ShowText("FORBUF_ROBOT_WAITTING_OPEN_DOOR");
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_OPEN_DOOR:     //doi mo cong
                    RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK);
                    if (RetState.DOOR_CTRL_SUCCESS == ret || robot.onForceGoToGate)
                    {
                        ds.resetWaitOpenBack();
                        robot.onForceGoToGate = false;
                        StateForkLift         = ForkLift.FORBUF_ROBOT_OPEN_DOOR_SUCCESS;
                        robot.ShowText("FORBUF_ROBOT_OPEN_DOOR_SUCCESS");
                    }
                    else if (RetState.DOOR_CTRL_ERROR == ret)
                    {
                        robot.ShowText("FORBUF_ROBOT_OPEN_DOOR_ERROR");
                        //StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION;
                        Thread.Sleep(1000);
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_OPEN_DOOR_SUCCESS:     // mo cua thang cong ,gui toa do line de robot di vao gap hang
                    // rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETUP);
                    if (rb.SendCmdAreaPallet(doorServiceCtrl.infoPallet))
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN;
                        robot.ShowText("FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN");
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN:     // doi robot gap hang
                    try
                    {
                        DoorStatus ret1 = ds.getStatusDoor(DoorType.DOOR_BACK);
                        if (DoorStatus.DOOR_OPEN == ret1)
                        {
                            robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
                            if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                            {
                                resCmd        = ResponseCommand.RESPONSE_NONE;
                                StateForkLift = ForkLift.FORBUF_ROBOT_FINISH_PALLET_UP;
                                Console.WriteLine("pallet ID" + order.palletId);

                                robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.GOING_OUTSIDE_GATE;
                                robot.ShowText("FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE");
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                            }
                        }
                        //else if (DoorStatus.DOOR_UNKNOW == ret1)
                        //{

                        //}
                        else    /* if(DoorStatus.DOOR_CLOSE == ret1)*/
                        {
                            robot.ShowText("DOOR_ERROR_(-___-)");
                            robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_STOP, true);
                            ds.setDoorBusy(true);
                            ds.openDoor(DoorType.DOOR_BACK);
                            Thread.Sleep(7000);
                        }
                    }
                    catch
                    {
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_FINISH_PALLET_UP:

                    String destName = Traffic.DetermineArea(endPointBuffer.Position, TypeZone.MAIN_ZONE);
                    if (destName.Equals("VIM"))
                    {
                        if (checkAnyRobotAtElevator(robot))
                        {
                            break;
                        }
                        else
                        {
                            StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
                        }
                    }
                    else
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE:
                    // kiem tra da toi vung dong cong
                    if (!Traffic.RobotIsInArea("CLOSE-GATE", robot.properties.pose.Position))
                    {
                        DoorStatus ret2 = ds.getStatusDoor(DoorType.DOOR_BACK);
                        if (DoorStatus.DOOR_OPEN == ret2)
                        {
                            robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                            {
                                Global_Object.setGateStatus(order.gate, false);
                                resCmd = ResponseCommand.RESPONSE_NONE;
                                ds.LampSetStateOff(DoorType.DOOR_FRONT);
                                ds.closeDoor(DoorType.DOOR_BACK);
                                ds.setDoorBusy(false);
                                StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE;
                                robot.ShowText("FORBUF_ROBOT_WAITTING_CLOSE_GATE");
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                            }
                        }
                        //else if (DoorStatus.DOOR_UNKNOW == ret1)
                        //{

                        //}
                        else    /* if(DoorStatus.DOOR_CLOSE == ret1)*/
                        {
                            robot.ShowText("DOOR_ERROR_(-___-)");
                            robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_STOP, true);
                            ds.setDoorBusy(true);
                            ds.openDoor(DoorType.DOOR_BACK);
                            Thread.Sleep(6000);
                        }
                    }
                    else
                    {
                        robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false);
                        if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                        {
                            Global_Object.setGateStatus(order.gate, false);
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            ds.LampSetStateOff(DoorType.DOOR_FRONT);
                            ds.closeDoor(DoorType.DOOR_BACK);
                            ds.setDoorBusy(false);
                            StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE;
                            robot.ShowText("FORBUF_ROBOT_WAITTING_CLOSE_GATE");
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE:
                    try
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_CHECK_GOTO_BUFFER_OR_MACHINE;
                        robot.SwitchToDetectLine(false);
                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                        registryRobotJourney.endPoint       = endPointBuffer.Position;
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_CHECK_GOTO_BUFFER_OR_MACHINE:
                    order.status    = StatusOrderResponseCode.DELIVERING;
                    flToMachineInfo = GetPriorityTaskForkLiftToMachine(order.productDetailId);
                    if (flToMachineInfo == null)
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_ZONE_BUFFER_READY;
                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                        registryRobotJourney.endPoint       = endPointBuffer.Position;
                    }
                    else
                    {
                        FreePlanedBuffer(order.palletId_P);
                        UpdatePalletState(PalletStatus.W, flToMachineInfo.palletId, flToMachineInfo.planId);
                        StateForkLift = ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM_REG;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_ZONE_BUFFER_READY:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    Pose destPos1 = endPointBuffer;
                    if (rb.SendPoseStamped(destPos1))
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                        robot.ShowText("FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM;
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            resCmd        = ResponseCommand.RESPONSE_NONE;
                            StateForkLift = ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER:
                    // xóa đăng ký vùng
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                    {
                        break;
                    }
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            resCmd        = ResponseCommand.RESPONSE_NONE;
                            StateForkLift = ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER:
                    JResult = FlToBuf.GetInfoPallet_P_InBuffer(PistonPalletCtrl.PISTON_PALLET_DOWN);
                    String data = JsonConvert.SerializeObject(JResult.jInfoPallet);
                    if (rb.SendCmdAreaPallet(data))
                    {
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER;
                        robot.ShowText("FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER");
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        FlToBuf.UpdatePalletState(PalletStatus.W, JResult.palletId, order.planId);
                        //   rb.SendCmdPosPallet (RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE);
                        StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER;
                        robot.ShowText("FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        Console.WriteLine("Loi Update :ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER");
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER:     // đợi
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        robot.bayId    = -1;
                        robot.bayIdReg = false;
                        robot.ReleaseWorkingZone();
                        robot.SwitchToDetectLine(false);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateForkLift = ForkLift.FORBUF_ROBOT_RELEASED;
                        robot.ShowText("FORBUF_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        Console.WriteLine("Loi Update :ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORBUF_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_FORKLIFT_TO_BUFFER;
                    ReleaseProcedureHandler(this);
                    ProRun = false;
                    robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.status = StatusOrderResponseCode.FINISHED;
                    break;

                case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM_REG:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateForkLift = ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM;
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    try
                    {
                        if (rb.SendPoseStamped(flToMachineInfo.frontLinePose))
                        {
                            StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE;
                            robot.ShowText("FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                            onFlagResetedGate = false;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE:
                    try
                    {
                        if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                        {
                            break;
                        }
                        if (rb.SendPoseStamped(flToMachineInfo.frontLinePose))
                        {
                            StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE;
                            robot.ShowText("FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                            onFlagResetedGate = false;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE:
                    try
                    {
                        TrafficRountineConstants.DetectRelease(registryRobotJourney);
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            StateForkLift = ForkLift.FORMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET_MACHINE;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET_MACHINE:
                    if (rb.SendCmdAreaPallet(flToMachineInfo.infoPallet))
                    {
                        StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE;
                        robot.ShowText("FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE");
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        // FlToBuf.UpdatePalletState(PalletStatus.W);
                        //   rb.SendCmdPosPallet (RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE);
                        StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE;
                        robot.ShowText("FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE:     // đợi
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        robot.SwitchToDetectLine(false);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateForkLift = ForkLift.FORMAC_ROBOT_RELEASED;
                        robot.ShowText("FORMAC_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLift.FORMAC_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    ds.removeListCtrlDoorBack();
                    TrafficRountineConstants.ReleaseAll(robot);
                    robot.orderItem = null;
                    // Global_Object.onFlagDoorBusy = false;
                    robot.SwitchToDetectLine(false);
                    // robot.robotTag = RobotStatus.IDLE;
                    robot.ReleaseWorkingZone();
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_FORKLIFT_TO_BUFFER;
                    // if (errorCode == ErrorCode.RUN_OK) {

                    // } else {
                    //     ErrorProcedureHandler (this);
                    // }
                    ProRun = false;
                    robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.status             = StatusOrderResponseCode.FINISHED;
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    ReleaseProcedureHandler(this);
                    break;

                case ForkLift.FORMAC_ROBOT_DESTROY:     // trả robot về robotmanagement để nhận quy trình mới
                    ds.removeListCtrlDoorBack();
                    TrafficRountineConstants.ReleaseAll(robot);
                    robot.SwitchToDetectLine(false);
                    robot.ReleaseWorkingZone();
                    robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.IDLE;
                    //robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
                    ProRun = false;
                    UpdateInformationInProc(this, ProcessStatus.F);
                    order.status      = StatusOrderResponseCode.ROBOT_ERROR;
                    selectHandleError = SelectHandleError.CASE_ERROR_EXIT;
                    procedureStatus   = ProcedureStatus.PROC_KILLED;
                    // RestoreOrderItem();
                    //FreePlanedBuffer(order.palletId_P);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                //////////////////////////////////////////////////////
                default:
                    break;
                }
                Thread.Sleep(700);
            }
            StateForkLift = ForkLift.FORBUF_IDLE;
        }
예제 #25
0
 public void RegistrationTranfficService(TrafficManagementService TrafficService)
 {
     this.TrafficService = TrafficService;
 }
        public void Procedure(object ojb)
        {
            ProcedureMachineToReturn BfToRe  = (ProcedureMachineToReturn)ojb;
            RobotUnity               rb      = BfToRe.robot;
            DataMachineToReturn      p       = BfToRe.points;
            TrafficManagementService Traffic = BfToRe.Traffic;

            rb.mcuCtrl.lampRbOn();
            robot.ShowText(" Start -> " + procedureCode);
            while (ProRun)
            {
                switch (StateMachineToReturn)
                {
                case MachineToReturn.MACRET_IDLE:
                    break;

                case MachineToReturn.MACRET_SELECT_BEHAVIOR_ONZONE:
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
                    {
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_BACK_FRONTLINE_READY;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(BfToRe.GetFrontLineMachine().Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = BfToRe.GetFrontLineMachine().Position;
                        }
                    }
                    else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position))
                    {
                        if (rb.SendPoseStamped(BfToRe.GetFrontLineMachine()))
                        {
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(BfToRe.GetFrontLineMachine().Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = BfToRe.GetFrontLineMachine().Position;
                            //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                        }
                    }
                    else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position))
                    {
                        Pose   destPos1  = BfToRe.GetFrontLineMachine();
                        String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE);
                        if (destName1.Equals("OUTER"))
                        {
                            //robot.ShowText("GO FRONTLINE IN OUTER");
                            if (rb.SendPoseStamped(destPos1))
                            {
                                StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1.Position;
                                //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                            }
                        }
                        else if (destName1.Equals("VIM"))
                        {
                            //robot.ShowText("GO FRONTLINE IN VIM");
                            if (rb.SendPoseStamped(destPos1))
                            {
                                StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1.Position;
                                //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                                //robot.ShowText("CHECK - REG");
                            }
                        }
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Start();
                        do
                        {
                            robot.onFlagGoBackReady = true;
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                            {
                                robot.onFlagGoBackReady = false;

                                Pose   destPos1  = BfToRe.GetFrontLineMachine();
                                String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE);
                                if (destName1.Equals("OUTER"))
                                {
                                    //robot.ShowText("GO FRONTLINE IN OUTER");
                                    if (rb.SendPoseStamped(destPos1))
                                    {
                                        resCmd = ResponseCommand.RESPONSE_NONE;
                                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos1.Position;
                                        //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                                    }
                                }
                                else if (destName1.Equals("VIM"))
                                {
                                    //robot.ShowText("GO FRONTLINE IN VIM");
                                    if (rb.SendPoseStamped(destPos1))
                                    {
                                        resCmd = ResponseCommand.RESPONSE_NONE;
                                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_READY;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos1.Position;
                                        //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                                        //robot.ShowText("CHECK - REG");
                                    }
                                }
                                break;
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            Thread.Sleep(100);
                        } while (ProRunStopW);
                        sw.Stop();
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_READY:
                    try
                    {
                        TrafficRountineConstants.DetectRelease(registryRobotJourney);
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (robot.ReachedGoal())
                        {
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM;
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM:
                    try
                    {
                        TrafficRountineConstants.DetectRelease(registryRobotJourney);
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (robot.ReachedGoal())
                        {
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE:
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (robot.ReachedGoal())
                        {
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE:
                    if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletMachine(PistonPalletCtrl.PISTON_PALLET_UP)))
                    {
                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE;
                        //robot.ShowText("MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE");
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;

                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE;
                        //robot.ShowText("MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE:     // đợi
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                        {
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_RETURN_SELECT_BEHAVIOR_ONZONE;
                            // cập nhật lại điểm xuất phát
                            registryRobotJourney.startPoint = robot.properties.pose.Position;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_GOTO_RETURN_SELECT_BEHAVIOR_ONZONE:
                    String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
                    Pose   destPos        = BfToRe.GetFrontLineReturn();
                    String destName       = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
                    if (startNamePoint.Equals("VIM"))
                    {
                        if (rb.SendPoseStamped(destPos))
                        {
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = destPos.Position;
                            //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                        }
                    }
                    else if (startNamePoint.Equals("OUTER"))
                    {
                        if (destName.Equals("OUTER"))
                        {
                            if (rb.SendPoseStamped(destPos))
                            {
                                StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                        else if (destName.Equals("VIM"))
                        {
                            if (rb.SendPoseStamped(destPos))
                            {
                                StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos.Position;
                                //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM:     // dang di
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if ( robot.ReachedGoal())
                        {
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN:     // dang di
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if ( robot.ReachedGoal())
                        {
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET:
                    if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletReturn(PistonPalletCtrl.PISTON_PALLET_DOWN)))
                    {
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_DROPDOWN_PALLET;
                        //robot.ShowText("MACRET_ROBOT_WAITTING_DROPDOWN_PALLET");
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_DROPDOWN_PALLET:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        BfToRe.UpdatePalletState(PalletStatus.W, order.palletId_P, order.palletId_P);
                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_GOTO_FRONTLINE;
                        //robot.ShowText("MACRET_ROBOT_WAITTING_GOTO_FRONTLINE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_WAITTING_GOTO_FRONTLINE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateMachineToReturn = MachineToReturn.MACRET_ROBOT_RELEASED;
                        //robot.ShowText("MACRET_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case MachineToReturn.MACRET_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    TrafficRountineConstants.ReleaseAll(robot);
                    Global_Object.onFlagRobotComingGateBusy = false;
                    robot.orderItem   = null;
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_MACHINE_TO_RETURN;
                    ReleaseProcedureHandler(this);
                    ProRun = false;
                    //robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.status             = StatusOrderResponseCode.FINISHED;
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                default:
                    break;
                }
                //robot.ShowText("-> " + procedureCode);
                Thread.Sleep(5);
            }
            StateMachineToReturn = MachineToReturn.MACRET_IDLE;
        }
        public void Procedure(object ojb)
        {
            ProcedureForkLiftToMachine FlToMach = (ProcedureForkLiftToMachine)ojb;
            RobotUnity rb = FlToMach.robot;

            ds = FlToMach.door;
            ds.setRb(rb);
            TrafficManagementService Traffic = FlToMach.Traffic;

            rb.mcuCtrl.lampRbOn();
            robot.ShowText(" Start -> " + procedureCode);
            while (ProRun)
            {
                switch (StateForkLiftToMachine)
                {
                case ForkLiftToMachine.FORMACH_IDLE:
                    robot.ShowText("FORMACH_IDLE");
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_GOTO_CHECKIN_GATE:     //gui toa do di den khu vuc checkin cong
                    if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                    {
                        if (false == robot.CheckInGateFromReadyZoneBehavior(ds.config.PointFrontLine.Position))
                        {
                            robot.robotTag         = RobotStatus.WORKING;
                            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_GOTO_BACK_FRONTLINE_READY;
                        }
                    }
                    else
                    {
                        robot.robotTag = RobotStatus.WORKING;
                        if (Traffic.RobotIsInArea("OPA4", rb.properties.pose.Position))
                        {
                            rb.SendPoseStamped(ds.config.PointFrontLine);
                            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE;
                            robot.ShowText("FORMACH_ROBOT_CAME_CHECKIN_GATE");
                        }
                        else
                        {
                            rb.SendPoseStamped(ds.config.PointCheckInGate);
                            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_CHECKIN_GATE;
                            robot.ShowText("FORMACH_ROBOT_WAITTING_GOTO_CHECKIN_GATE");
                        }
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_LEFT))
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Start();
                        do
                        {
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                            {
                                if (rb.SendPoseStamped(ds.config.PointFrontLine))
                                {
                                    resCmd = ResponseCommand.RESPONSE_NONE;
                                    StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE;
                                    robot.ShowText("FORMACH_ROBOT_WAITTING_GOTO_GATE");
                                    break;
                                }
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            Thread.Sleep(100);
                        } while (ProRunStopW);
                        sw.Stop();
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_CHECKIN_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    //if (robot.ReachedGoal())
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        rb.UpdateRiskAraParams(0, rb.properties.L2, rb.properties.WS, rb.properties.DistInter);
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_CAME_CHECKIN_GATE;
                        robot.ShowText("FORMACH_ROBOT_CAME_CHECKIN_GATE");
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_CAME_CHECKIN_GATE:     // đã đến vị trí, kiem tra va cho khu vuc cong san sang de di vao.
                                                                            // robot.ShowText( "FORMACH_ROBOT_WAITTING_GOTO_GATE ===> FLAG " + Traffic.HasRobotUnityinArea(ds.config.PointFrontLine.Position));
                    if (false == robot.CheckInZoneBehavior(ds.config.PointFrontLine.Position))
                    {
                        if (TrafficRountineConstants.RegIntZone_READY.ProcessRegistryIntersectionZone(robot))
                        {
                            rb.UpdateRiskAraParams(40, rb.properties.L2, rb.properties.WS, rb.properties.DistInter);
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                            rb.SendPoseStamped(ds.config.PointFrontLine);
                            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE;
                            robot.ShowText("FORMACH_ROBOT_WAITTING_GOTO_GATE");
                        }
                        else
                        {
                            Thread.Sleep(500);
                            break;
                        }
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE:
                    if (Traffic.RobotIsInArea("C3", rb.properties.pose.Position))
                    {
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE_OPENDOOR;
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOTO_GATE_OPENDOOR:
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        TrafficRountineConstants.RegIntZone_READY.Release(robot);
                        robot.SwitchToDetectLine(true);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_CAME_GATE_POSITION;
                        robot.ShowText("FORMACH_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_CAME_GATE_POSITION:     // da den khu vuc cong , gui yeu cau mo cong.
                    robot.robotRegistryToWorkingZone.onRobotwillCheckInsideGate = false;
                    //ds.setDoorBusy(true);
                    //ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_OPEN_DOOR;
                    robot.ShowText("FORMACH_ROBOT_WAITTING_OPEN_DOOR");
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_OPEN_DOOR:     //doi mo cong
                    RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK);
                    if (ret == RetState.DOOR_CTRL_SUCCESS)
                    {
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_OPEN_DOOR_SUCCESS;
                        robot.ShowText("FORMACH_ROBOT_OPEN_DOOR_SUCCESS");
                    }
                    else if (ret == RetState.DOOR_CTRL_ERROR)
                    {
                        robot.ShowText("FORMACH_ROBOT_OPEN_DOOR_ERROR");
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_CAME_GATE_POSITION;
                        Thread.Sleep(1000);
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_OPEN_DOOR_SUCCESS:     // mo cua thang cong ,gui toa do line de robot di vao gap hang
                    // rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETUP);
                    rb.SendCmdAreaPallet(ds.config.infoPallet);
                    StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_PICKUP_PALLET_IN;
                    robot.ShowText("FORMACH_ROBOT_WAITTING_PICKUP_PALLET_IN");
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_PICKUP_PALLET_IN:     // doi robot gap hang
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        // FlToMach.UpdatePalletState(PalletStatus.F);
                        StateForkLiftToMachine    = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
                        robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.GOING_OUTSIDE_GATE;
                        robot.ShowText("FORMACH_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        Global_Object.setGateStatus(order.gate, false);
                        robot.SwitchToDetectLine(false);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        ds.LampSetStateOff(DoorService.DoorType.DOOR_FRONT);
                        ds.closeDoor(DoorService.DoorType.DOOR_BACK);
                        ds.setDoorBusy(false);
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_CLOSE_GATE;
                        robot.ShowText("FORMACH_ROBOT_WAITTING_CLOSE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_CLOSE_GATE:     // doi dong cong.
                    try
                    {
                        if (TrafficRountineConstants.RegIntZone_READY.ProcessRegistryIntersectionZone(robot))
                        {
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                            if (rb.SendPoseStamped(FlToMach.GetFrontLineMachine()))
                            {
                                Global_Object.setGateStatus(order.gate, false);
                                // Global_Object.onFlagDoorBusy = false;
                                StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE;
                                robot.ShowText("FORMACH_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE");
                            }
                        }
                        else
                        {
                            Thread.Sleep(500);
                            break;
                        }
                        //}
                        //else
                        //{
                        //    // errorCode = ErrorCode.CLOSE_DOOR_ERROR;
                        //    // CheckUserHandleError(this);
                        //}
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE:
                    try
                    {
                        // Global_Object.onFlagDoorBusy = false;
                        if (!Traffic.HasRobotUnityinArea("GATE_CHECKOUT", robot))
                        {
                            //  robot.ShowText("RELEASED ZONE");
                            if (!onFlagResetedGate)
                            {
                                TrafficRountineConstants.RegIntZone_READY.Release(robot);
                                robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.IDLE;
                                onFlagResetedGate         = true;
                                Global_Object.onFlagRobotComingGateBusy = false;
                                robot.ReleaseWorkingZone();
                            }
                        }
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        //if (robot.ReachedGoal())
                        {
                            //robot.TurnOnCtrlSelfTraffic(false);
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                            rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_DROPDOWN_PALLET);
                            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE;
                            robot.ShowText("FORMACH_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE");
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_DROPDOWN_PALLET)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_RELEASED;
                        robot.ShowText("FORMACH_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                //case ForkLiftToMachine.FORMACH_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE: // đợi
                //    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                //    {
                //        resCmd = ResponseCommand.RESPONSE_NONE;
                //        rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                //        StateForkLiftToMachine = ForkLiftToMachine.FORMACH_ROBOT_RELEASED;
                //        robot.ShowText("FORMACH_ROBOT_RELEASED");
                //    }
                //    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                //    {
                //        errorCode = ErrorCode.DETECT_LINE_ERROR;
                //        CheckUserHandleError(this);
                //    }
                //    break;
                case ForkLiftToMachine.FORMACH_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    ds.removeListCtrlDoorBack();
                    robot.orderItem = null;
                    //Global_Object.onFlagDoorBusy = false;
                    robot.SwitchToDetectLine(false);
                    //      robot.robotTag = RobotStatus.IDLE;
                    robot.ReleaseWorkingZone();

                    rb.PreProcedureAs = ProcedureControlAssign.PRO_FORKLIFT_TO_MACHINE;
                    order.status      = StatusOrderResponseCode.FINISHED;
                    // if (errorCode == ErrorCode.RUN_OK) {
                    ReleaseProcedureHandler(this);
                    // } else {
                    //     ErrorProcedureHandler (this);
                    // }
                    ProRun = false;
                    robot.ShowText("RELEASED");
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                default:
                    break;
                }
                Thread.Sleep(5);
            }
            StateForkLiftToMachine = ForkLiftToMachine.FORMACH_IDLE;
        }
예제 #28
0
 public void Registry(TrafficManagementService trafficManagementService)
 {
     this.trafficManagementService = trafficManagementService;
 }
        public void Procedure(object ojb)
        {
            ProcedureBufferToGate BuffToGate = (ProcedureBufferToGate)ojb;
            RobotUnity            rb         = BuffToGate.robot;

            // DataBufferToGate p = BuffToGate.points;
            doorServiceCtrl = getDoorService();
            ds = doorServiceCtrl.doorService;
            ds.setRb(rb);
            TrafficManagementService Traffic = BuffToGate.Traffic;

            rb.mcuCtrl.lampRbOn();

            robot.ShowText(" Start -> " + procedureCode);
            while (ProRun)
            {
                switch (StateBufferToGate)
                {
                case BufferToGate.BUFGATE_IDLE:
                    break;

                case BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE:
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS))
                    {
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_GOTO_BACK_FRONTLINE_READY;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = BuffToGate.GetFrontLineBuffer().Position;
                            ////robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY");
                            ////robot.ShowText("CHECK - REG");
                        }
                    }
                    else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
                    {
                        if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                        {
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                            registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
                            registryRobotJourney.startPoint     = robot.properties.pose.Position;
                            registryRobotJourney.endPoint       = BuffToGate.GetFrontLineBuffer().Position;
                            ////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                        }
                    }
                    else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
                    {
                        Point  destPos1  = BuffToGate.GetFrontLineBuffer().Position;
                        String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE);
                        if (destName1.Equals("OUTER"))
                        {
                            ////robot.ShowText("GO FRONTLINE IN OUTER");
                            if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                            {
                                StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1;
                                ////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                        else if (destName1.Equals("VIM"))
                        {
                            ////robot.ShowText("GO FRONTLINE IN VIM");
                            if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                            {
                                StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG;
                                registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                registryRobotJourney.endPoint       = destPos1;
                                ////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                ////robot.ShowText("CHECK - REG");
                            }
                        }
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    ////robot.ShowText("START :GOTO_BACK_FRONTLINE_READY");
                    if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
                    {
                        Stopwatch sw = new Stopwatch();
                        sw.Start();
                        do
                        {
                            robot.onFlagGoBackReady = true;
                            if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                            {
                                robot.onFlagGoBackReady = false;

                                Pose   destPos2  = BuffToGate.GetFrontLineBuffer();
                                String destName2 = Traffic.DetermineArea(destPos2.Position, TypeZone.MAIN_ZONE);
                                if (destName2.Equals("OUTER"))
                                {
                                    ////robot.ShowText("GO FRONTLINE IN OUTER");
                                    if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                                    {
                                        resCmd            = ResponseCommand.RESPONSE_NONE;
                                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos2.Position;
                                        ////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                    }
                                }
                                else if (destName2.Equals("VIM"))
                                {
                                    ////robot.ShowText("GO FRONTLINE IN VIM");
                                    if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                                    {
                                        resCmd            = ResponseCommand.RESPONSE_NONE;
                                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_READY;
                                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
                                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                                        registryRobotJourney.endPoint       = destPos2.Position;
                                        ////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                                        ////robot.ShowText("CHECK - REG");
                                    }
                                }
                                break;
                            }
                            else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
                            {
                                errorCode = ErrorCode.DETECT_LINE_ERROR;
                                CheckUserHandleError(this);
                                break;
                            }
                            Thread.Sleep(100);
                        } while (ProRunStopW);
                        sw.Stop();
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_ZONE_BUFFER_READY:     // doi khu vuc buffer san sang de di vao
                    try
                    {
                        if (false == robot.CheckInZoneBehavior(BuffToGate.GetFrontLineBuffer().Position))
                        {
                            if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
                            {
                                StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                                ////robot.ShowText("BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
                            }
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_READY:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            resCmd            = ResponseCommand.RESPONSE_NONE;
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG:

                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM;
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                    {
                        break;
                    }
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            resCmd            = ResponseCommand.RESPONSE_NONE;
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER:
                    try
                    {
                        if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                        {
                            break;
                        }
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            resCmd            = ResponseCommand.RESPONSE_NONE;
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
                        }
                        else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                        {
                            errorCode = ErrorCode.DETECT_LINE_ERROR;
                            CheckUserHandleError(this);
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER:
                    String palletInfo = JsonConvert.SerializeObject(BuffToGate.GetInfoOfPalletBuffer(PistonPalletCtrl.PISTON_PALLET_UP));
                    if (rb.SendCmdAreaPallet(palletInfo))
                    {
                        // rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETUP);
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER;
                        ////robot.ShowText("BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER");
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        BuffToGate.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId);
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER;
                        ////robot.ShowText("BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER:     // đợi
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        robot.bayId    = -1;
                        robot.bayIdReg = false;
                        resCmd         = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateBufferToGate = BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE_TO_GATE;
                        // cập nhật lại điểm xuất phát
                        registryRobotJourney.startPoint = robot.properties.pose.Position;
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE_TO_GATE:
                    String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
                    Pose   destPos        = doorServiceCtrl.PointFrontLine;
                    String destName       = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
                    // đi tới đầu line cổng theo tọa độ chỉ định. gate 1 , 2, 3
                    if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine))
                    {
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG;
                        // Cap Nhat Thong Tin CHuyen Di
                        registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
                        registryRobotJourney.startPoint     = robot.properties.pose.Position;
                        registryRobotJourney.endPoint       = doorServiceCtrl.PointFrontLine.Position;
                        ////robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE");
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG:
                    if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
                    {
                        break;
                    }
                    else
                    {
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM;
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (Traffic.RobotIsInArea("C5", rb.properties.pose.Position))
                    {
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR;
                    }

                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR:
                    TrafficRountineConstants.DetectRelease(registryRobotJourney);
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION;
                        ////robot.ShowText("BUFGATE_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION;
                        ////robot.ShowText("BUFGATE_ROBOT_CAME_GATE_POSITION");
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION:     // da den khu vuc cong , gui yeu cau mo cong.
                    //  ds.setDoorBusy(true);
                    //   ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_OPEN_DOOR;
                    ////robot.ShowText("BUFGATE_ROBOT_WAITTING_OPEN_DOOR");
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_OPEN_DOOR:     //doi mo cong
                    RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK);
                    if (ret == RetState.DOOR_CTRL_SUCCESS)
                    {
                        if (rb.SendCmdAreaPallet(doorServiceCtrl.infoPallet))
                        {
                            StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER;
                            ////robot.ShowText("BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER");
                        }
                    }
                    else if (ret == RetState.DOOR_CTRL_ERROR)
                    {
                        robot.ShowText("BUFGATE_ROBOT_WAITTING_OPEN_DOOR_ERROR__(-_-)");
                        Thread.Sleep(1000);
                        ds.setDoorBusy(true);
                        ds.openDoor(DoorService.DoorType.DOOR_BACK);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER:     // doi robot gap hang
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        // BuffToGate.UpdatePalletState(PalletStatus.W);
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
                        ////robot.ShowText("BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        ds.closeDoor(DoorService.DoorType.DOOR_BACK);
                        ds.setDoorBusy(false);
                        ds.LampSetStateOn(DoorType.DOOR_BACK);
                        StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CLOSE_GATE;
                        ////robot.ShowText("BUFGATE_ROBOT_WAITTING_CLOSE_GATE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToGate.BUFGATE_ROBOT_WAITTING_CLOSE_GATE:     // doi dong cong.
                    StateBufferToGate = BufferToGate.BUFGATE_ROBOT_RELEASED;
                    ////robot.ShowText("BUFGATE_ROBOT_WAITTING_CLOSE_GATE");
                    break;

                case BufferToGate.BUFGATE_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới
                    ds.removeListCtrlDoorBack();
                    robot.bayId    = -1;
                    robot.bayIdReg = false;
                    TrafficRountineConstants.ReleaseAll(robot);
                    robot.robotTag    = RobotStatus.IDLE;
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_GATE;
                    ReleaseProcedureHandler(this);
                    ProRun = false;
                    UpdateInformationInProc(this, ProcessStatus.S);
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    KillEvent();
                    break;

                case BufferToGate.BUFGATE_ROBOT_DESTROY:
                    ds.removeListCtrlDoorBack();
                    ProRunStopW              = false;
                    ProRunStopW              = false;
                    robot.robotTag           = RobotStatus.IDLE;
                    ProRun                   = false;
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    TrafficRountineConstants.ReleaseAll(robot);
                    break;

                default:
                    break;
                }
                //robot.ShowText("-> " + procedureCode);
                Thread.Sleep(500);
            }
            StateBufferToGate = BufferToGate.BUFGATE_IDLE;
        }
예제 #30
0
 public TrafficProcedureService(RobotUnity robot, TrafficManagementService trafficService) : base(robot)
 {
     this.traffic = trafficService;
     this.robot   = robot;
 }