Example #1
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;
        }
        public void Procedure(object ojb)
        {
            ProcedureBufferToMachine BfToMa = (ProcedureBufferToMachine)ojb;
            RobotUnity rb = BfToMa.robot;
            // DataBufferToMachine p = BfToMa.points;
            TrafficManagementService Traffic = BfToMa.Traffic;

            robot.ShowText(" Start -> " + procedureCode);
            //GetFrontLineBuffer(false);
            //  ProRun = false;
            rb.mcuCtrl.lampRbOn();

            /* frontLinePose = BfToMa.GetFrontLineBuffer();
             * if (frontLinePose == null)
             * {
             *   robot.bayId = -1;
             *   TrafficRountineConstants.ReleaseAll(robot);
             *   robot.bayId = -1;
             *   robot.bayIdReg = false;
             *   robot.orderItem = null;
             *   robot.SwitchToDetectLine(false);
             *   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_BUFFER_TO_MACHINE;
             *   ReleaseProcedureHandler(this);
             *   ProRun = false;
             *   UpdateInformationInProc(this, ProcessStatus.S);
             *   order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
             *   order.endTimeProcedure = DateTime.Now;
             *   order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
             *   SaveOrderItem(order);
             *   KillEvent();
             * }
             * else
             * {
             *   order.frontLinePos = frontLinePose.Position;
             *   robot.bayId = bayId;
             * }*/
            while (ProRun)
            {
                switch (StateBufferToMachine)
                {
                case BufferToMachine.BUFMAC_IDLE:
                    //robot.ShowText("BUFMAC_IDLE");
                    break;

                case BufferToMachine.BUFMAC_GET_FRONTLINE:
                    frontLinePose = BfToMa.GetFrontLineBuffer();
                    if (frontLinePose == null)
                    {
                        if (countFrontLineNull++ < 10)
                        {
                            break;
                        }
                        countFrontLineNull = 0;
                        robot.bayId        = -1;
                        TrafficRountineConstants.ReleaseAll(robot);
                        robot.bayId     = -1;
                        robot.bayIdReg  = false;
                        robot.orderItem = null;
                        robot.SwitchToDetectLine(false);
                        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_BUFFER_TO_MACHINE;
                        }
                        ReleaseProcedureHandler(this);
                        ProRun = false;
                        UpdateInformationInProc(this, ProcessStatus.S);
                        order.status             = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
                        order.endTimeProcedure   = DateTime.Now;
                        order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                        SaveOrderItem(order);
                        KillEvent();
                    }
                    else
                    {
                        order.frontLinePos   = frontLinePose.Position;
                        robot.bayId          = bayId;
                        StateBufferToMachine = BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE;
                    }
                    //robot.ShowText("BUFMAC_IDLE");
                    break;

                case BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE:
                    if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS))
                    {
                        if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
                        {
                            StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY;
                            //robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY");
                            //robot.ShowText("CHECK - REG");
                        }
                    }
                    else if (Traffic.RobotIsInArea("GATE", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
                    {
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                    }
                    else     //if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position,TypeZone.MAIN_ZONE))
                    {
                        Point  destPos1  = frontLinePose.Position;
                        String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE);
                        if (rb.SendPoseStamped(frontLinePose))
                        {
                            StateBufferToMachine = BufferToMachine.BUFMAC_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");
                        }
                    }

                    break;

                case BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY:
                    if (Traffic.HasOtherRobotUnityinArea("GATE", robot))
                    {
                        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;

                                Point  destPos2  = frontLinePose.Position;
                                String destName2 = Traffic.DetermineArea(destPos2, TypeZone.MAIN_ZONE);
                                //robot.ShowText("GO FRONTLINE IN OUTER");
                                if (rb.SendPoseStamped(frontLinePose))
                                {
                                    resCmd = ResponseCommand.RESPONSE_NONE;
                                    StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
                                }
                                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 BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER:
                    // nếu chưa đến goal phải check tới vùng c1
                    // nếu goal nằm trong C1 thì không đăng ký

                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;

                            StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
                        }
                        else
                        {
                            if (!Traffic.PositionIsInArea(frontLinePose.Position).Equals("C1"))
                            {
                                if (TrafficRountineConstants.DetetectInsideStationCheckMerz(registryRobotJourney))
                                {
                                    break;
                                }
                            }
                            if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
                            {
                                break;
                            }
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER:
                    JPallet jPallet_H = BfToMa.GetInfoPallet_H_InBuffer(PistonPalletCtrl.PISTON_PALLET_UP);
                    jPResult = BfToMa.GetInfoOfPalletBuffer_Compare_W_H(PistonPalletCtrl.PISTON_PALLET_UP, jPallet_H);
                    String data = JsonConvert.SerializeObject(jPResult.jInfoPallet);
                    if (rb.SendCmdAreaPallet(data))
                    {
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER;
                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER");
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        BfToMa.UpdatePalletState(PalletStatus.F, jPResult.palletId, order.planId);
                        //Reset trạng thai W cho pallet_H nếu truoc đó có cây W
                        if (jPResult.jInfoPallet.row < order.palletRow)
                        {
                            BfToMa.UpdatePalletState(PalletStatus.W, order.palletId_H, order.planId);
                        }
                        onUpdatedPalletState = true;
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER;
                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER:     // đợi
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                        {
                            robot.bayId    = -1;
                            robot.bayIdReg = false;
                            robot.ReleaseWorkingZone();
                            robot.SwitchToDetectLine(false);
                            resCmd = ResponseCommand.RESPONSE_NONE;
                            StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE;
                            // cập nhật lại điểm xuất phát
                            registryRobotJourney.startPoint = robot.properties.pose.Position;
                            //robot.ShowText("BUFMAC_ROBOT_GOTO_FRONTLINE_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 BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE:
                    String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
                    Pose   destPos        = BfToMa.GetFrontLineMachine();
                    String destName       = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
                    if (rb.SendPoseStamped(destPos))
                    {
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET;
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET:     // tai may
                    try
                    {
                        if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
                        {
                            robot.SwitchToDetectLine(true);
                            StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET;
                        }
                    }
                    catch (System.Exception)
                    {
                        errorCode = ErrorCode.CAN_NOT_GET_DATA;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET:
                    if (rb.SendCmdAreaPallet(BfToMa.GetInfoOfPalletMachine(PistonPalletCtrl.PISTON_PALLET_DOWN)))
                    {
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET;
                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET");
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET:
                    if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
                    {
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE;
                        //robot.ShowText("BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE:
                    if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
                    {
                        robot.SwitchToDetectLine(false);
                        resCmd = ResponseCommand.RESPONSE_NONE;
                        //rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
                        StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED;
                        //robot.ShowText("BUFMAC_ROBOT_RELEASED");
                    }
                    else if (resCmd == ResponseCommand.RESPONSE_ERROR)
                    {
                        errorCode = ErrorCode.DETECT_LINE_ERROR;
                        CheckUserHandleError(this);
                    }
                    break;

                case BufferToMachine.BUFMAC_ROBOT_RELEASED:     // trả robot về robotmanagement để nhận quy trình mới

                    TrafficRountineConstants.ReleaseAll(robot);
                    robot.bayId     = -1;
                    robot.bayIdReg  = false;
                    robot.orderItem = null;
                    robot.SwitchToDetectLine(false);
                    // Release WorkinZone Robot
                    //   robot.robotTag = RobotStatus.IDLE;
                    rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_MACHINE;
                    // if (errorCode == ErrorCode.RUN_OK) {
                    ReleaseProcedureHandler(this);
                    // } 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;
                    SaveOrderItem(order);
                    KillEvent();
                    break;

                case BufferToMachine.BUFMAC_ROBOT_DESTROY:
                    order.endTimeProcedure   = DateTime.Now;
                    order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
                    SaveOrderItem(order);
                    robot.SwitchToDetectLine(false);
                    robot.ReleaseWorkingZone();
                    // StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED;
                    //robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
                    ProRun = false;
                    UpdateInformationInProc(this, ProcessStatus.F);
                    order.status = StatusOrderResponseCode.ROBOT_ERROR;
                    //reset status pallet Faile H->Ws
                    selectHandleError = SelectHandleError.CASE_ERROR_EXIT;
                    procedureStatus   = ProcedureStatus.PROC_KILLED;
                    //FreeHoldBuffer(order.palletId_H);
                    KillEvent();

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

                default:
                    break;
                }
                //   robot.ShowText("-> " + procedureCode);
                Thread.Sleep(700);
            }
            StateBufferToMachine = BufferToMachine.BUFMAC_IDLE;
        }