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) { 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; }
public void Procedure(object ojb) { ProcedureBufferToReturn BfToRe = (ProcedureBufferToReturn)ojb; RobotUnity rb = BfToRe.robot; TrafficManagementService Traffic = BfToRe.Traffic; rb.mcuCtrl.lampRbOn(); robot.ShowText(" Start -> " + procedureCode); while (ProRun) { switch (StateBufferToReturn) { case BufferToReturn.BUFRET_IDLE: robot.ShowText("BUFRET_IDLE"); break; case BufferToReturn.BUFRET_ROBOT_GOTO_CHECKIN_BUFFER: // bắt đầu rời khỏi vùng GATE đi đến check in/ đảm bảo check out vùng cổng để robot kế tiếp vào làm việc 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(BfToRe.GetCheckInBuffer_Return(order.bufferId))) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER; robot.ShowText("BUFRET_ROBOT_WAITTING_GOTO_CHECKIN_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(); } } else { if (rb.SendPoseStamped(BfToRe.GetCheckInBuffer_Return(order.bufferId))) { StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER; robot.ShowText("BUFRET_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER"); } } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER: // doi robot di den khu vuc checkin cua vung buffer if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (rb.checkNewPci()) { //if (robot.ReachedGoal(rb.getPointCheckInConfirm())) //{ resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_ZONE_BUFFER_READY; robot.ShowText("BUFRET_ROBOT_WAITTING_ZONE_BUFFER_READY"); //} } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_ZONE_BUFFER_READY: // doi khu vuc buffer san sang de di vao try { if (false == robot.CheckInZoneBehavior(BfToRe.GetAnyPointInBuffer_Return(order.bufferId).Position)) { //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; if (rb.SendPoseStamped(BfToRe.GetFrontLineBuffer())) { StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; robot.ShowText("BUFRET_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER: try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (robot.ReachedGoal()) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER: if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletBuffer_Return(PistonPalletCtrl.PISTON_PALLET_UP, order.bufferId))) { StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_PICKUP_PALLET_BUFFER; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; robot.ShowText("BUFRET_ROBOT_WAITTING_PICKUP_PALLET_BUFFER"); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_PICKUP_PALLET_BUFFER: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; BfToRe.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId); StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER; robot.ShowText("BUFRET_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER: // đợi try { if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.ReleaseWorkingZone(); robot.SwitchToDetectLine(false); //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; if (rb.SendPoseStamped(BfToRe.GetCheckInReturn())) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_GOTO_CHECKIN_RETURN; robot.ShowText("BUFRET_ROBOT_GOTO_CHECKIN_RETURN"); } } 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 BufferToReturn.BUFRET_ROBOT_GOTO_CHECKIN_RETURN: // dang di 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); StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_CAME_CHECKIN_RETURN; robot.ShowText("BUFRET_ROBOT_CAME_CHECKIN_RETURN"); } break; case BufferToReturn.BUFRET_ROBOT_CAME_CHECKIN_RETURN: // đã đến vị trí try { if (false == robot.CheckInZoneBehavior(BfToRe.GetFrontLineReturn().Position)) { Global_Object.onFlagRobotComingGateBusy = true; rb.UpdateRiskAraParams(40, rb.properties.L2, rb.properties.WS, rb.properties.DistInter); //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; if (rb.SendPoseStamped(BfToRe.GetFrontLineReturn())) { StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET; robot.ShowText("BUFRET_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET"); } } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET: try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (robot.ReachedGoal()) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_SEND_CMD_DROPDOWN_PALLET; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; // case BufferToReturn.BUFRET_ROBOT_CAME_FRONTLINE_DROPDOWN_PALLET: // đang trong tiến trình dò line và thả pallet // rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETDOWN); // StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_POINT_DROP_PALLET; // break; // case BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_POINT_DROP_PALLET: // if (true == rb.CheckPointDetectLine(BfToRe.GetPointPallet(), rb)) // { // rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_LINEDETECT_COMING_POSITION); // StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_DROPDOWN_PALLET; // } // break; case BufferToReturn.BUFRET_ROBOT_SEND_CMD_DROPDOWN_PALLET: if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletReturn(PistonPalletCtrl.PISTON_PALLET_DOWN))) { StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_DROPDOWN_PALLET; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; robot.ShowText("BUFRET_ROBOT_WAITTING_DROPDOWN_PALLET"); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_DROPDOWN_PALLET: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN) { resCmd = ResponseCommand.RESPONSE_NONE; BfToRe.UpdatePalletState(PalletStatus.W, order.palletId_H, order.planId); StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_FRONTLINE; robot.ShowText("BUFRET_ROBOT_WAITTING_GOTO_FRONTLINE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_WAITTING_GOTO_FRONTLINE: if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_RELEASED; robot.ShowText("BUFRET_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToReturn.BUFRET_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới Global_Object.onFlagRobotComingGateBusy = false; robot.orderItem = null; robot.SwitchToDetectLine(false); // robot.robotTag = RobotStatus.IDLE; robot.ReleaseWorkingZone(); rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_RETURN; // 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; KillEvent(); break; default: break; } Thread.Sleep(5); } StateBufferToReturn = BufferToReturn.BUFRET_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; }
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); robot.ShowText("Error Data Request FrontLine Buffer" + order.dataRequest); 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; } ProRun = false; robot.ShowText("RELEASED BF---"); UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); ReleaseProcedureHandler(this); } 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; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = frontLinePose.Position; //robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY"); //robot.ShowText("CHECK - REG"); } } else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position, TypeZone.MAIN_ZONE)) { //robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : VIM"); if (rb.SendPoseStamped(frontLinePose)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = frontLinePose.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); //robot.ShowText("CHECK - REG"); } } else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position, TypeZone.MAIN_ZONE)) { Point destPos1 = frontLinePose.Position; String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE); //robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : OUTER"); if (destName1.Equals("OUTER")) { //robot.ShowText("COME FRONTLINE : OUTER"); 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"); } } else if (destName1.Equals("VIM")) { //robot.ShowText("COME FRONTLINE : OUTER"); if (rb.SendPoseStamped(frontLinePose)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_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"); } } } break; case BufferToMachine.BUFMAC_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_RIGHT)) { 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; Point destPos2 = frontLinePose.Position; String destName2 = Traffic.DetermineArea(destPos2, TypeZone.MAIN_ZONE); if (destName2.Equals("OUTER")) { //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(frontLinePose)) { resCmd = ResponseCommand.RESPONSE_NONE; 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 = destPos2; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (destName2.Equals("VIM")) { //robot.ShowText("GO FRONTLINE IN VIM"); if (rb.SendPoseStamped(frontLinePose)) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_FROM_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos2; //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 BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER_VIM: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { TrafficRountineConstants.DetectRelease(registryRobotJourney); } if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY; //robot.ShowText("BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY"); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER: // doi robot di den khu vuc checkin cua vung buffer if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY; //robot.ShowText("BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY"); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY: // doi khu vuc buffer san sang de di vao try { if (false == robot.CheckInZoneBehavior(BfToMa.GetAnyPointInBuffer().Position)) { if (rb.SendPoseStamped(frontLinePose)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_FROM_READY: TrafficRountineConstants.DetectRelease(registryRobotJourney); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM; } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM: TrafficRountineConstants.DetectRelease(registryRobotJourney); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } 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; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER: // dò và release vùng đăng ky if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } TrafficRountineConstants.DetectRelease(registryRobotJourney); 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; } } 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 (startNamePoint.Equals("VIM")) { if (rb.SendPoseStamped(destPos)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_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)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET; 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)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_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 BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM; } break; case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM: try { TrafficRountineConstants.DetectRelease(registryRobotJourney); 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_GOTO_FRONTLINE_DROPDOWN_PALLET: 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) { // } 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 BufferToMachine.BUFMAC_ROBOT_DESTROY: order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; 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; }