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; }