public TrafficProcedureService(RobotUnity robot, TrafficManagementService trafficService) : base(robot) { this.traffic = trafficService; this.robot = robot; }
public TrafficProcedureService(RobotUnity robot) : base(robot) { this.robot = robot; }
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; }
public void setRb(RobotUnity robot) { this.rb = robot; }
public bool AssignWaitTask(OrderItem order) { processAssignAnTaskWait = ProcessAssignAnTaskWait.PROC_ANY_GET_ANROBOT_IN_WAITTASKLIST; OrderItem orderItem_wait = null; RobotUnity robotwait = null; while (true) { switch (processAssignAnTaskWait) { case ProcessAssignAnTaskWait.PROC_ANY_IDLE: break; case ProcessAssignAnTaskWait.PROC_ANY_GET_ANROBOT_IN_WAITTASKLIST: if (robotManageService.RobotUnityWaitTaskList.Count > 0) { ResultRobotReady result = robotManageService.GetRobotUnityWaitTaskItem0(); if (result != null) { robotwait = result.robot; if (result.onReristryCharge) { procedureService.Register(ProcedureItemSelected.PROCEDURE_ROBOT_TO_READY, robotwait, null); robotManageService.RemoveRobotUnityWaitTaskList(robotwait); } else { if (order != null) { processAssignAnTaskWait = ProcessAssignAnTaskWait.PROC_ANY_CHECK_HAS_ANTASK; break; } else { processAssignAnTaskWait = ProcessAssignAnTaskWait.PROC_ANY_CHECK_ROBOT_GOTO_READY; // mở lại break; } } } } return(false); case ProcessAssignAnTaskWait.PROC_ANY_CHECK_HAS_ANTASK: orderItem_wait = order; //orderItem_wait.onAssiged = true; processAssignAnTaskWait = ProcessAssignAnTaskWait.PROC_ANY_ASSIGN_ANTASK; orderItem_wait.robot = robotwait.properties.Label; robotwait.orderItem = orderItem_wait; // MoveElementToEnd(); break; case ProcessAssignAnTaskWait.PROC_ANY_CHECK_ROBOT_GOTO_READY: robotwait.TurnOnSupervisorTraffic(true); procedureService.Register(ProcedureItemSelected.PROCEDURE_ROBOT_TO_READY, robotwait, null); Console.WriteLine(robotwait.properties.Label + " Assign goto ready_____________(-_-)______________"); robotManageService.RemoveRobotUnityWaitTaskList(robotwait); return(false); case ProcessAssignAnTaskWait.PROC_ANY_ASSIGN_ANTASK: robotwait.TurnOnSupervisorTraffic(true); SelectProcedureItem(robotwait, orderItem_wait); deviceItemsList[0].RemoveFirstOrder(); robotManageService.RemoveRobotUnityWaitTaskList(robotwait); processAssignAnTaskWait = ProcessAssignAnTaskWait.PROC_ANY_GET_ANROBOT_IN_WAITTASKLIST; orderItem_wait.status = StatusOrderResponseCode.DELIVERING; return(true); } Thread.Sleep(10); } }
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) { 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 bool AssignTaskAtReady(OrderItem order) { processAssignTaskReady = ProcessAssignTaskReady.PROC_READY_GET_ANROBOT_INREADYLIST; OrderItem orderItem_ready = null; RobotUnity robotatready = null; while (true) { switch (processAssignTaskReady) { case ProcessAssignTaskReady.PROC_READY_IDLE: break; case ProcessAssignTaskReady.PROC_READY_GET_ANROBOT_INREADYLIST: if (robotManageService.RobotUnityReadyList.Count > 0) { ResultRobotReady result = robotManageService.GetRobotUnityReadyItem0(); if (result != null) { robotatready = result.robot; if ((true == result.onReristryCharge) || (robotatready.properties.enableChage == true)) { procedureService.Register(ProcedureItemSelected.PROCEDURE_ROBOT_TO_CHARGE, robotatready, null); robotManageService.RemoveRobotUnityReadyList(robotatready); } else { if (order != null) { if (!trafficService.HasOtherRobotUnityinArea("READY", robotatready)) { if (order.typeReq == TyeRequest.TYPEREQUEST_FORLIFT_TO_BUFFER || order.typeReq == TyeRequest.TYPEREQUEST_FORLIFT_TO_MACHINE) { if (trafficService.HasOtherRobotUnityinArea("GATE", robotatready)) { // MoveElementToEnd(); } else { if (TrafficRountineConstants.RegIntZone_READY.ProcessRegistryIntersectionZone(robotatready)) { processAssignTaskReady = ProcessAssignTaskReady.PROC_READY_CHECK_HAS_ANTASK; break; } else { TrafficRountineConstants.RegIntZone_READY.Release(robotatready); } } } else { if (TrafficRountineConstants.RegIntZone_READY.ProcessRegistryIntersectionZone(robotatready)) { processAssignTaskReady = ProcessAssignTaskReady.PROC_READY_CHECK_HAS_ANTASK; break; } else { TrafficRountineConstants.RegIntZone_READY.Release(robotatready); } } } } } } } return(false); case ProcessAssignTaskReady.PROC_READY_CHECK_HAS_ANTASK: orderItem_ready = order; // orderItem_ready.onAssiged = true; Console.WriteLine(processAssignTaskReady); orderItem_ready.robot = robotatready.properties.Label; robotatready.orderItem = orderItem_ready; processAssignTaskReady = ProcessAssignTaskReady.PROC_READY_ASSIGN_ANTASK; // MoveElementToEnd(); break; case ProcessAssignTaskReady.PROC_READY_ASSIGN_ANTASK: TrafficRountineConstants.RegIntZone_READY.Release(robotatready); robotatready.TurnOnSupervisorTraffic(true); Console.WriteLine(processAssignTaskReady); SelectProcedureItem(robotatready, orderItem_ready); deviceItemsList[0].RemoveFirstOrder(); robotManageService.RemoveRobotUnityReadyList(robotatready); orderItem_ready.status = StatusOrderResponseCode.DELIVERING; processAssignTaskReady = ProcessAssignTaskReady.PROC_READY_CHECK_ROBOT_OUTSIDEREADY; return(true); } Thread.Sleep(10); } }
public static void ReleaseAll(RobotUnity robot, String nameException) { RegIntZone_READY.Release(robot); RegIntZone_GATEZONE.Release(robot); }
public bool Contains(RobotUnity robot) { return(Registryrobotlist.Contains(robot)); }
public static void ReleaseAll(RobotUnity robot) { RegIntZone_READY.Release(robot); RegIntZone_GATEZONE.Release(robot); }
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 DBProcedureService(RobotUnity robot) { }
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) { ProcedureRobotToCharger RbToChar = (ProcedureRobotToCharger)ojb; RobotUnity rb = RbToChar.robot; ErrorCodeCharger result; rb.properties.enableChage = true; robot.ShowText(" Start -> " + procedureCode); while (ProRun) { switch (StateRobotToCharge) { case RobotGoToCharge.ROBCHAR_IDLE: //robot.ShowText("ROBCHAR_IDLE"); break; #if USE_AUTO_CHARGE // case RobotGoToCharge.ROBCHAR_CHARGER_CHECKSTATUS: // if(true == chargerCtrl.WaitState(ChargerState.ST_READY,TIME_OUT_WAIT_STATE)){ // StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT; // } // break; //kiểm tra kết nối và trạng thái sạc case RobotGoToCharge.ROBCHAR_ROBOT_GOTO_CHARGER: //robot.ShowText("ROBCHAR_ROBOT_GOTO_CHARGER"); robot.TurnOnSupervisorTraffic(false); if (rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_GETIN_CHARGER)) { //Thread.Sleep(1000); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT; robot.ShowText("ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT"); } break; case RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT: try { if (resCmd == ResponseCommand.RESPONSE_FINISH_DETECTLINE_GETIN_CHARGER) { if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_TURNOFF_PC)) { rb.Dispose(); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC; sw.Start(); robot.ShowText("ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC"); } } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_CHARGER_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; CheckUserHandleError(this); } break; //cho phép cắt nguồn robot case RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC: if (true != rb.properties.IsConnected) { rb.Dispose(); robot.ShowText("Sleep 30s waitting turnoff power"); Thread.Sleep(30000); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_START_CHARGE; sw.Reset(); robot.ShowText("ROBCHAR_ROBOT_START_CHARGE"); } else { if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_TURNOFF_PC) { sw.Reset(); errorCode = ErrorCode.CAN_NOT_TURN_OFF_PC; CheckUserHandleError(this); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT; } } break; case RobotGoToCharge.ROBCHAR_ROBOT_START_CHARGE: try { if (true == chargerCtrl.StartCharge()) { StateRobotToCharge = RobotGoToCharge.ROBCHAR_WAITTING_ROBOT_CONTACT_CHARGER; robot.ShowText("ROBCHAR_WAITTING_ROBOT_CONTACT_CHARGER"); } else { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; CheckUserHandleError(this); } break; case RobotGoToCharge.ROBCHAR_WAITTING_ROBOT_CONTACT_CHARGER: try { result = chargerCtrl.WaitState(ChargerState.ST_CHARGING, TIME_OUT_WAIT_STATE); if (ErrorCodeCharger.TRUE == result) { StateRobotToCharge = RobotGoToCharge.ROBCHAR_WAITTING_CHARGEBATTERY; robot.ShowText("ROBCHAR_WAITTING_CHARGEBATTERY"); } else { if (result == ErrorCodeCharger.ERROR_CONNECT) { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; robot.ShowText("CONNECT_CHARGER_ERROR"); } else { errorCode = ErrorCode.CONTACT_CHARGER_ERROR; robot.ShowText("CONTACT_CHARGER_ERROR"); } //CheckUserHandleError(this); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_START_CHARGE; } } catch (System.Exception) { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; robot.ShowText("CONNECT_CHARGER_ERROR"); //CheckUserHandleError(this); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_START_CHARGE; } break; //robot tiep xuc tram sac //case RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT: // rb.SendCmdPosPallet (RequestCommandPosPallet.REQUEST_TURNOFF_PC); // StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC; // //robot.ShowText("ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC"); // sw.Start (); // break; //cho phép cắt nguồn robot //case RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_CUTOFF_POWER_PC: // if (true != rb.properties.IsConnected) { // StateRobotToCharge = RobotGoToCharge.ROBCHAR_WAITTING_CHARGEBATTERY; // //robot.ShowText("ROBCHAR_WAITTING_CHARGEBATTERY"); // } else { // if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_TURNOFF_PC) { // sw.Stop (); // errorCode = ErrorCode.CAN_NOT_TURN_OFF_PC; // CheckUserHandleError (this); // StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_ALLOW_CUTOFF_POWER_ROBOT; // } // } // break; #endif case RobotGoToCharge.ROBCHAR_WAITTING_CHARGEBATTERY: #if false //for test StateRobotToCharge = RobotGoToCharge.ROBCHAR_FINISHED_CHARGEBATTERY; #else try { timeCountGetBatLevel++; if (timeCountGetBatLevel >= TIME_COUNT_GET_BAT_LEVEL) { timeCountGetBatLevel = 0; result = rb.mcuCtrl.GetBatteryLevel(ref batLevel); Console.WriteLine("=================****+++++bat level {0}+++++++++++++++++++", batLevel.data[0]); if (ErrorCodeCharger.TRUE == result) { rb.properties.BatteryLevelRb = (float)batLevel.data[0]; #if USE_AUTO_CHARGE if ((batLevel.data[0] >= BATTERY_FULL_LEVEL) || (rb.properties.enableChage == false)) #else if (batLevel.data[0] >= BATTERY_NEW_BAT) #endif { //Thread.Sleep((int)TIME_DELAY_RELEASE_CHARGE); #if USE_AUTO_CHARGE StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_STOP_CHARGE; #else StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_RECONNECTING; #endif robot.ShowText("ROBCHAR_ROBOT_STOP_CHARGE"); } } else { #if USE_AUTO_CHARGE if (result == ErrorCodeCharger.ERROR_CONNECT) { errorCode = ErrorCode.CONNECT_BOARD_CTRL_ROBOT_ERROR; } CheckUserHandleError(this); #endif } } } catch (System.Exception e) { Console.WriteLine(e); #if USE_AUTO_CHARGE errorCode = ErrorCode.CONNECT_BOARD_CTRL_ROBOT_ERROR; CheckUserHandleError(this); #endif } #endif break; //dợi charge battery và thông tin giao tiếp server và trạm sạc #if USE_AUTO_CHARGE case RobotGoToCharge.ROBCHAR_ROBOT_STOP_CHARGE: try { if (true == chargerCtrl.StopCharge()) { robot.ShowText("Stop charger success"); StateRobotToCharge = RobotGoToCharge.ROBCHAR_FINISHED_CHARGEBATTERY; robot.ShowText("ROBCHAR_FINISHED_CHARGEBATTERY"); } else { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; //CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CONNECT_CHARGER_ERROR; //CheckUserHandleError(this); } break; case RobotGoToCharge.ROBCHAR_FINISHED_CHARGEBATTERY: Thread.Sleep(10000); try { if (true == rb.mcuCtrl.TurnOnPcRobot()) { robot.ShowText("Turn on pc"); Thread.Sleep(60000); robot.ShowText("Reconnect server"); rb.Start(rb.properties.Url); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_RECONNECTING; robot.ShowText("ROBCHAR_ROBOT_WAITTING_RECONNECTING"); } else { errorCode = ErrorCode.CAN_NOT_TURN_ON_PC; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CONNECT_BOARD_CTRL_ROBOT_ERROR; CheckUserHandleError(this); } break; //Hoàn Thành charge battery và thông tin giao tiếp server và trạm sạc #endif case RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_RECONNECTING: if (true == CheckReconnectServer(TIME_OUT_ROBOT_RECONNECT_SERVER)) { #if USE_AUTO_CHARGE StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_GETOUT_CHARGER; #else StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_RELEASED; #endif robot.ShowText("ROBCHAR_ROBOT_GETOUT_CHARGER"); } else { errorCode = ErrorCode.ROBOT_CANNOT_CONNECT_SERVER_AFTER_CHARGE; CheckUserHandleError(this); } break; //Robot mở nguồng và đợi connect lại #if USE_AUTO_CHARGE case RobotGoToCharge.ROBCHAR_ROBOT_GETOUT_CHARGER: if (rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_GETOUT_CHARGER)) { sw.Restart(); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_GETOUT_CHARGER; robot.ShowText("ROBCHAR_ROBOT_WAITTING_GETOUT_CHARGER"); } break; case RobotGoToCharge.ROBCHAR_ROBOT_WAITTING_GETOUT_CHARGER: if (resCmd == ResponseCommand.RESPONSE_FINISH_DETECTLINE_GETOUT_CHARGER) { StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_RELEASED; robot.ShowText("ROBCHAR_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_CHARGER_ERROR; CheckUserHandleError(this); } if (sw.ElapsedMilliseconds >= 30000) { sw.Reset(); StateRobotToCharge = RobotGoToCharge.ROBCHAR_ROBOT_GETOUT_CHARGER; } break; #endif case RobotGoToCharge.ROBCHAR_ROBOT_RELEASED: robot.robotTag = RobotStatus.IDLE; rb.PreProcedureAs = ProcedureControlAssign.PRO_READY; ReleaseProcedureHandler(this); ProRun = false; robot.ShowText("RELEASED"); UpdateInformationInProc(this, ProcessStatus.S); order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); break; // trả robot về robotmanagement để nhận quy trình mới default: break; } //robot.ShowText("-> " + procedureCode); Thread.Sleep(100); } StateRobotToCharge = RobotGoToCharge.ROBCHAR_IDLE; }
public void Register(ProcedureItemSelected ProcedureItem, RobotUnity robot, OrderItem orderItem) { CleanUp(); switch (ProcedureItem) { case ProcedureItemSelected.PROCEDURE_FORLIFT_TO_BUFFER: { ProcedureForkLiftToBuffer procfb = new ProcedureForkLiftToBuffer(robot, doorService, trafficService); procfb.Registry(deviceService); ProcedureDataItems profbDataItems = new ProcedureDataItems(); profbDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocfb = new RegisterProcedureItem() { item = procfb, robot = robot, procedureDataItems = profbDataItems }; procfb.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procfb.ErrorProcedureHandler += ErrorApprearInProcedureItem; //RegisterProcedureItemList.Add (itemprocfb); procfb.AssignAnOrder(orderItem); robot.proRegistryInRobot.pFB = procfb; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_FORKLIFT_TO_BUFFER; procfb.Registry(robotManagementService); procfb.Start(); break; } case ProcedureItemSelected.PROCEDURE_BUFFER_TO_MACHINE: ProcedureBufferToMachine procbm = new ProcedureBufferToMachine(robot, trafficService); procbm.Registry(deviceService); ProcedureDataItems prcobmDataItems = new ProcedureDataItems(); prcobmDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocbm = new RegisterProcedureItem() { item = procbm, robot = robot, procedureDataItems = prcobmDataItems }; procbm.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procbm.ErrorProcedureHandler += ErrorApprearInProcedureItem; //RegisterProcedureItemList.Add (itemprocbm); procbm.AssignAnOrder(orderItem); robot.proRegistryInRobot.pBM = procbm; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_BUFFER_TO_MACHINE; procbm.Registry(robotManagementService); procbm.Start(); break; case ProcedureItemSelected.PROCEDURE_BUFFER_TO_RETURN: ProcedureBufferToReturn procbr = new ProcedureBufferToReturn(robot, trafficService); ProcedureDataItems prcobrDataItems = new ProcedureDataItems(); prcobrDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocbr = new RegisterProcedureItem() { item = procbr, robot = robot, procedureDataItems = prcobrDataItems }; procbr.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procbr.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocbr); procbr.AssignAnOrder(orderItem); robot.proRegistryInRobot.pBR = procbr; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_BUFFER_TO_RETURN; procbr.Registry(robotManagementService); procbr.Start(); break; case ProcedureItemSelected.PROCEDURE_PALLETEMPTY_MACHINE_TO_RETURN: ProcedureMachineToReturn procmr = new ProcedureMachineToReturn(robot, trafficService); ProcedureDataItems prcomrDataItems = new ProcedureDataItems(); prcomrDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocmr = new RegisterProcedureItem() { item = procmr, robot = robot, procedureDataItems = prcomrDataItems }; procmr.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procmr.ErrorProcedureHandler += ErrorApprearInProcedureItem; //RegisterProcedureItemList.Add (itemprocmr); procmr.AssignAnOrder(orderItem); robot.proRegistryInRobot.pMR = procmr; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_MACHINE_TO_RETURN; procmr.Registry(robotManagementService); procmr.Start(); break; case ProcedureItemSelected.PROCEDURE_RETURN_TO_GATE: ProcedureReturnToGate procrg = new ProcedureReturnToGate(robot, doorService, trafficService); ProcedureDataItems prorgDataItems = new ProcedureDataItems(); prorgDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocrg = new RegisterProcedureItem() { item = procrg, robot = robot, procedureDataItems = prorgDataItems }; procrg.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procrg.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrg); procrg.AssignAnOrder(orderItem); procrg.Registry(robotManagementService); procrg.Start(); break; case ProcedureItemSelected.PROCEDURE_ROBOT_TO_CHARGE: ProcedureRobotToCharger procrc = new ProcedureRobotToCharger(robot, chargerService, robot.properties.ChargeID); ProcedureDataItems procrcDataItems = new ProcedureDataItems(); procrcDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocrc = new RegisterProcedureItem() { item = procrc, robot = robot, procedureDataItems = procrcDataItems }; procrc.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procrc.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrc); robot.proRegistryInRobot.pRC = procrc; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_CHARGE; procrc.Start(); break; case ProcedureItemSelected.PROCEDURE_ROBOT_TO_READY: robot.ShowText(robot.properties.Label + "Add PROCEDURE_ROBOT_TO_READY ----------(--___--)---------------"); ProcedureRobotToReady procrr = new ProcedureRobotToReady(robot, robot.properties.ChargeID, trafficService, chargerService, null); ProcedureDataItems procrrDataItems = new ProcedureDataItems(); procrrDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocrr = new RegisterProcedureItem() { item = procrr, robot = robot, procedureDataItems = procrrDataItems }; procrr.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procrr.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrr); robot.proRegistryInRobot.pRR = procrr; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_READY; procrr.Registry(deviceService); procrr.Registry(robotManagementService); procrr.Registry(assigmentTask); procrr.Start(); break; case ProcedureItemSelected.PROCEDURE_ROBOT_READY_TO_READY: ProcedureRobotToReady procRrr = new ProcedureRobotToReady(robot, robot.properties.ChargeID, trafficService, chargerService, doorService.DoorMezzamineUp.config.PointCheckInGate); ProcedureDataItems procRrrDataItems = new ProcedureDataItems(); procRrrDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocRrr = new RegisterProcedureItem() { item = procRrr, robot = robot, procedureDataItems = procRrrDataItems }; procRrr.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procRrr.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add(itemprocRrr); robot.proRegistryInRobot.pRR = procRrr; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_READY; procRrr.Registry(robotManagementService); procRrr.Start(); break; case ProcedureItemSelected.PROCEDURE_FORLIFT_TO_MACHINE: ProcedureForkLiftToMachine procfm = new ProcedureForkLiftToMachine(robot, doorService, trafficService); ProcedureDataItems profmDataItems = new ProcedureDataItems(); profmDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocfm = new RegisterProcedureItem() { item = procfm, robot = robot, procedureDataItems = profmDataItems }; procfm.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procfm.ErrorProcedureHandler += ErrorApprearInProcedureItem; //RegisterProcedureItemList.Add(itemprocfm); procfm.AssignAnOrder(orderItem); robot.proRegistryInRobot.pFM = procfm; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_FORKLIFT_TO_MACHINE; procfm.Registry(robotManagementService); procfm.Start(); break; case ProcedureItemSelected.PROCEDURE_BUFFER_TO_BUFFER: ProcedureBufferReturnToBuffer401 procbb = new ProcedureBufferReturnToBuffer401(robot, trafficService); ProcedureDataItems prcobbDataItems = new ProcedureDataItems(); prcobbDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocbb = new RegisterProcedureItem() { item = procbb, robot = robot, procedureDataItems = prcobbDataItems }; procbb.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procbb.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocbr); procbb.AssignAnOrder(orderItem); robot.proRegistryInRobot.pBB = procbb; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_BUFFER_TO_BUFFER; procbb.Registry(robotManagementService); procbb.Start(); break; case ProcedureItemSelected.PROCEDURE_BUFFER_TO_GATE: ProcedureBufferToGate procbg = new ProcedureBufferToGate(robot, doorService, trafficService); ProcedureDataItems probgDataItems = new ProcedureDataItems(); probgDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocbg = new RegisterProcedureItem() { item = procbg, robot = robot, procedureDataItems = probgDataItems }; procbg.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procbg.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrg); procbg.AssignAnOrder(orderItem); robot.proRegistryInRobot.pBG = procbg; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_BUFFER_TO_GATE; procbg.Registry(robotManagementService); procbg.Start(); break; case ProcedureItemSelected.PROCEDURE_MACHINE_TO_GATE: ProcedureMachineToGate procmg = new ProcedureMachineToGate(robot, doorService, trafficService); ProcedureDataItems promgDataItems = new ProcedureDataItems(); promgDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocmg = new RegisterProcedureItem() { item = procmg, robot = robot, procedureDataItems = promgDataItems }; procmg.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procmg.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrg); procmg.AssignAnOrder(orderItem); robot.proRegistryInRobot.pMG = procmg; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_MACHINE_TO_GATE; procmg.Start(); break; case ProcedureItemSelected.PROCEDURE_MACHINE_TO_BUFFER_RETURN: ProcedureMachineToBufferReturn procmbr = new ProcedureMachineToBufferReturn(robot, trafficService); ProcedureDataItems prombrDataItems = new ProcedureDataItems(); prombrDataItems.StartTaskTime = DateTime.Now; RegisterProcedureItem itemprocmbr = new RegisterProcedureItem() { item = procmbr, robot = robot, procedureDataItems = prombrDataItems }; procmbr.ReleaseProcedureHandler += ReleaseProcedureItemHandler; procmbr.ErrorProcedureHandler += ErrorApprearInProcedureItem; // RegisterProcedureItemList.Add (itemprocrg); procmbr.AssignAnOrder(orderItem); robot.proRegistryInRobot.pMBR = procmbr; robot.ProcedureRobotAssigned = ProcedureControlAssign.PRO_MACHINE_TO_BUFFER_RETURN; procmbr.Registry(robotManagementService); procmbr.Start(); break; } }
public void RegisteAnItem(ProcedureControlServices item, ProcedureDataItems procedureDataItems, RobotUnity robot) { }
public void AssignTaskGoToReady(RobotUnity robot) { procedureService.Register(ProcedureItemSelected.PROCEDURE_ROBOT_TO_READY, robot, null); }
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) { ProcedureMachineToReturn BfToRe = (ProcedureMachineToReturn)ojb; RobotUnity rb = BfToRe.robot; DataMachineToReturn p = BfToRe.points; TrafficManagementService Traffic = BfToRe.Traffic; rb.mcuCtrl.lampRbOn(); robot.ShowText(" Start -> " + procedureCode); while (ProRun) { switch (StateMachineToReturn) { case MachineToReturn.MACRET_IDLE: break; case MachineToReturn.MACRET_SELECT_BEHAVIOR_ONZONE: if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) { if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_BACK_FRONTLINE_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(BfToRe.GetFrontLineMachine().Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = BfToRe.GetFrontLineMachine().Position; } } else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position)) { if (rb.SendPoseStamped(BfToRe.GetFrontLineMachine())) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(BfToRe.GetFrontLineMachine().Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = BfToRe.GetFrontLineMachine().Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position)) { Pose destPos1 = BfToRe.GetFrontLineMachine(); String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE); if (destName1.Equals("OUTER")) { //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(destPos1)) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); } } else if (destName1.Equals("VIM")) { //robot.ShowText("GO FRONTLINE IN VIM"); if (rb.SendPoseStamped(destPos1)) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); //robot.ShowText("CHECK - REG"); } } } break; case MachineToReturn.MACRET_ROBOT_GOTO_BACK_FRONTLINE_READY: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT)) { Stopwatch sw = new Stopwatch(); sw.Start(); do { robot.onFlagGoBackReady = true; if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.onFlagGoBackReady = false; Pose destPos1 = BfToRe.GetFrontLineMachine(); String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE); if (destName1.Equals("OUTER")) { //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(destPos1)) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); } } else if (destName1.Equals("VIM")) { //robot.ShowText("GO FRONTLINE IN VIM"); if (rb.SendPoseStamped(destPos1)) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); //robot.ShowText("CHECK - REG"); } } break; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } Thread.Sleep(100); } while (ProRunStopW); sw.Stop(); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_READY: try { TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (robot.ReachedGoal()) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM; } break; case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE_FROM_VIM: try { TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (robot.ReachedGoal()) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE: try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (robot.ReachedGoal()) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_SEND_CMD_CAME_FRONTLINE_MACHINE: if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletMachine(PistonPalletCtrl.PISTON_PALLET_UP))) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE; //robot.ShowText("MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE"); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_PICKUP_PALLET_MACHINE: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE; //robot.ShowText("MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE: // đợi try { if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_RETURN_SELECT_BEHAVIOR_ONZONE; // cập nhật lại điểm xuất phát registryRobotJourney.startPoint = robot.properties.pose.Position; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_GOTO_RETURN_SELECT_BEHAVIOR_ONZONE: String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE); Pose destPos = BfToRe.GetFrontLineReturn(); String destName = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE); if (startNamePoint.Equals("VIM")) { if (rb.SendPoseStamped(destPos)) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (startNamePoint.Equals("OUTER")) { if (destName.Equals("OUTER")) { if (rb.SendPoseStamped(destPos)) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (destName.Equals("VIM")) { if (rb.SendPoseStamped(destPos)) { StateMachineToReturn = MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } } break; case MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN_FROM_VIM: // dang di if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if ( robot.ReachedGoal()) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_GOTO_FRONTLINE_RETURN: // dang di try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if ( robot.ReachedGoal()) { resCmd = ResponseCommand.RESPONSE_NONE; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_SEND_CMD_DROPDOWN_PALLET: if (rb.SendCmdAreaPallet(BfToRe.GetInfoOfPalletReturn(PistonPalletCtrl.PISTON_PALLET_DOWN))) { //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_DROPDOWN_PALLET; //robot.ShowText("MACRET_ROBOT_WAITTING_DROPDOWN_PALLET"); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_DROPDOWN_PALLET: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN) { resCmd = ResponseCommand.RESPONSE_NONE; BfToRe.UpdatePalletState(PalletStatus.W, order.palletId_P, order.palletId_P); StateMachineToReturn = MachineToReturn.MACRET_ROBOT_WAITTING_GOTO_FRONTLINE; //robot.ShowText("MACRET_ROBOT_WAITTING_GOTO_FRONTLINE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_WAITTING_GOTO_FRONTLINE: if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; StateMachineToReturn = MachineToReturn.MACRET_ROBOT_RELEASED; //robot.ShowText("MACRET_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case MachineToReturn.MACRET_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới TrafficRountineConstants.ReleaseAll(robot); Global_Object.onFlagRobotComingGateBusy = false; robot.orderItem = null; rb.PreProcedureAs = ProcedureControlAssign.PRO_MACHINE_TO_RETURN; ReleaseProcedureHandler(this); ProRun = false; //robot.ShowText("RELEASED"); UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.FINISHED; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); break; default: break; } //robot.ShowText("-> " + procedureCode); Thread.Sleep(5); } StateMachineToReturn = MachineToReturn.MACRET_IDLE; }
public McuCtrl(RobotUnity rb) : base(rb.properties.ipMcuCtrl, rb.properties.portMcuCtrl) { //this.stateCtrlLampRb = stateCtrlLamp.LAMP_MCU_IDLE; ctrlLampOnRbThread = new Thread(this.lampOnRbCtrlProcess); ctrlLampOnRbThread.Start(this); }
public void Procedure(object ojb) { ProcedureBufferReturnToBuffer401 BfToBf = (ProcedureBufferReturnToBuffer401)ojb; RobotUnity rb = BfToBf.robot; TrafficManagementService Traffic = BfToBf.Traffic; robot.ShowText(" Start -> " + procedureCode); rb.mcuCtrl.lampRbOn(); while (ProRun) { switch (StateBufferToBuffer) { case BufferToBuffer.BUFTOBUF_IDLE: //robot.ShowText("BUFTOBUF_IDLE"); break; case BufferToBuffer.BUFTOBUF_SELECT_BEHAVIOR_ONZONE_BUFFER_A: if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) { Pose endPose = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn); if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_GOTO_BACK_FRONTLINE_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = endPose.Position; } } else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position)) { Pose endPose = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn); if (rb.SendPoseStamped(endPose)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = endPose.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position)) { Pose destPos1 = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn); String destName1 = Traffic.DetermineArea(destPos1.Position, TypeZone.MAIN_ZONE); if (destName1.Equals("OUTER")) { //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(destPos1)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (destName1.Equals("VIM")) { //robot.ShowText("GO FRONTLINE IN VIM"); if (rb.SendPoseStamped(destPos1)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); //robot.ShowText("CHECK - REG"); } } } break; case BufferToBuffer.BUFTOBUF_ROBOT_GOTO_BACK_FRONTLINE_READY: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT)) { Stopwatch sw = new Stopwatch(); sw.Start(); do { robot.onFlagGoBackReady = true; if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.onFlagGoBackReady = false; Pose destPos2 = BfToBf.GetFrontLineBufferReturn_BRB401(order.dataRequest_BufferReturn); String destName2 = Traffic.DetermineArea(destPos2.Position, TypeZone.MAIN_ZONE); if (destName2.Equals("OUTER")) { //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(destPos2)) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos2.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (destName2.Equals("VIM")) { //robot.ShowText("GO FRONTLINE IN VIM"); if (rb.SendPoseStamped(destPos2)) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos2.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); //robot.ShowText("CHECK - REG"); } } break; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } Thread.Sleep(100); } while (ProRunStopW); sw.Stop(); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_READY: TrafficRountineConstants.DetectRelease(registryRobotJourney); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } try { //bool onComePoint2 = robot.ReachedGoal(); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (onComePoint2) { // robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM; } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A_FROM_VIM: TrafficRountineConstants.DetectRelease(registryRobotJourney); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_A: try { //bool onComePoint2 = robot.ReachedGoal(); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) //if (onComePoint2) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER_A: if (rb.SendCmdAreaPallet(BfToBf.GetInfoOfPalletBufferReturn_BRB401(PistonPalletCtrl.PISTON_PALLET_UP, order.dataRequest_BufferReturn))) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_PICKUP_PALLET_BUFFER_A; } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_PICKUP_PALLET_BUFFER_A: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; BfToBf.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId); onUpdatedPalletState = true; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A; //robot.ShowText("BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_A: // đợi try { if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.bayId = -1; robot.bayIdReg = false; robot.ReleaseWorkingZone(); robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_BUFFER_B_SELECT_ZONE; // cập nhật lại điểm xuất phát registryRobotJourney.startPoint = robot.properties.pose.Position; //robot.ShowText("BUFTOBUF_ROBOT_WAITTING_GOTO_FRONTLINE_BUFFER_B"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_BUFFER_B_SELECT_ZONE: String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE); Pose destPos = BfToBf.GetFrontLineBuffer401_BRB401(order.dataRequest_Buffer401); String destName = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE); if (startNamePoint.Equals("VIM")) { if (rb.SendPoseStamped(destPos)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (startNamePoint.Equals("OUTER")) { if (destName.Equals("OUTER")) { if (rb.SendPoseStamped(destPos)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } else if (destName.Equals("VIM")) { if (rb.SendPoseStamped(destPos)) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos.Position; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM_REG: if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM; } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B_FROM_VIM: try { TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_B: try { if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_SEND_CMD_CAME_FRONTLINE_BUFFER_B_FROM_VIM: if (rb.SendCmdAreaPallet(BfToBf.GetInfoOfPalletBuffer401_BRB401(PistonPalletCtrl.PISTON_PALLET_DOWN, order.dataRequest_Buffer401))) { StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B; robot.ShowText("BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B"); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER_B: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; BfToBf.UpdatePalletState(PalletStatus.W); //onUpdatedPalletState = true; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B; //robot.ShowText("FBUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER_B: // đợi try { if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.bayId = -1; robot.bayIdReg = false; robot.ReleaseWorkingZone(); robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_RELEASED; //robot.ShowText("BUFTOBUF_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToBuffer.BUFTOBUF_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới robot.bayId = -1; TrafficRountineConstants.ReleaseAll(robot); robot.orderItem = null; robot.SwitchToDetectLine(false); rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_BUFFER; ReleaseProcedureHandler(this); ProRun = false; UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.FINISHED; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); break; case BufferToBuffer.BUFTOBUF_ROBOT_DESTROY: TrafficRountineConstants.ReleaseAll(robot); order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; robot.SwitchToDetectLine(false); robot.ReleaseWorkingZone(); // StateBufferToBuffer = BufferToBuffer.BUFTOBUF_ROBOT_RELEASED; //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; //reset status pallet Faile H->Ws if (!onUpdatedPalletState) { BfToBf.UpdatePalletState(PalletStatus.F, order.palletId_P, order.planId); } selectHandleError = SelectHandleError.CASE_ERROR_EXIT; procedureStatus = ProcedureStatus.PROC_KILLED; FreeHoldBuffer(order.palletId_H); KillEvent(); //this.robot.DestroyRegistrySolvedForm(); break; default: break; } Thread.Sleep(5); } StateBufferToBuffer = BufferToBuffer.BUFTOBUF_IDLE; }
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; }