public void Destroy() { // Global_Object.onFlagDoorBusy = false; robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.IDLE; if (ds != null) { ds.LampSetStateOff(DoorType.DOOR_FRONT); ds.setDoorBusy(false); ds.removeListCtrlDoorBack(); } Global_Object.setGateStatus(order.gate, false); ProRunStopW = false; robot.orderItem = null; robot.SwitchToDetectLine(false); robot.robotTag = RobotStatus.IDLE; robot.ReleaseWorkingZone(); order.status = StatusOrderResponseCode.ROBOT_ERROR; //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; selectHandleError = SelectHandleError.CASE_ERROR_EXIT; this.robot.DestroyRegistrySolvedForm(); procedureStatus = ProcedureStatus.PROC_KILLED; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; // RestoreOrderItem(); }
public void Destroy() { // StateBufferToReturn = BufferToReturn.BUFRET_ROBOT_RELEASED; ProRunStopW = false; robot.orderItem = null; robot.SwitchToDetectLine(false); robot.robotTag = RobotStatus.IDLE; robot.ReleaseWorkingZone(); //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; selectHandleError = SelectHandleError.CASE_ERROR_EXIT; }
public override void FinishStatesCallBack(Int32 message) { this.resCmd = (ResponseCommand)message; if (this.resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.ReleaseWorkingZone(); } }
public void Destroy() { if (ds != null) { ds.LampSetStateOff(DoorType.DOOR_BACK); ds.setDoorBusy(false); ds.removeListCtrlDoorBack(); } ProRunStopW = false; robot.orderItem = null; robot.SwitchToDetectLine(false); robot.robotTag = RobotStatus.IDLE; robot.ReleaseWorkingZone(); //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; selectHandleError = SelectHandleError.CASE_ERROR_EXIT; }
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) { ProcedureForkLiftToBuffer FlToBuf = (ProcedureForkLiftToBuffer)ojb; RobotUnity rb = FlToBuf.robot; doorServiceCtrl = new DoorServiceCtrl(); doorServiceCtrl = getDoorService(); ds = doorServiceCtrl.doorService; ds.setRb(rb); TrafficManagementService Traffic = FlToBuf.Traffic; ForkLiftToMachineInfo flToMachineInfo = new ForkLiftToMachineInfo(); rb.mcuCtrl.lampRbOn(); robot.ShowText(" Start -> " + procedureCode); /* endPointBuffer = FlToBuf.GetFrontLineBuffer(true); * if (endPointBuffer == null) * { * robot.bayId = -1; * Console.WriteLine("Error Data Request" + order.dataRequest); * order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE; * TrafficRountineConstants.ReleaseAll(robot); * robot.orderItem = null; * robot.SwitchToDetectLine(false); * robot.ReleaseWorkingZone(); * if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) * { * TrafficRountineConstants.RegIntZone_READY.Release(robot); * robot.robotTag = RobotStatus.IDLE; * robot.SetSafeYellowcircle(false); * robot.SetSafeBluecircle(false); * robot.SetSafeSmallcircle(false); * robot.TurnOnSupervisorTraffic(false); * // rb.mcuCtrl.lampRbOff(); * procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY; * } * else * procedureCode = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER; * ReleaseProcedureHandler(this); * ProRun = false; * robot.ShowText("RELEASED"); * UpdateInformationInProc(this, ProcessStatus.S); * order.endTimeProcedure = DateTime.Now; * order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; * SaveOrderItem(order); * KillEvent(); * } * else * { * order.frontLinePos = endPointBuffer.Position; * robot.bayId = bayId; * }*/ while (ProRun) { // JPallet aa= GetInfoPallet_P_InBuffer(TrafficRobotUnity.PistonPalletCtrl.PISTON_PALLET_DOWN); switch (StateForkLift) { case ForkLift.FORBUF_IDLE: robot.ShowText("FORBUF_IDLE"); break; case ForkLift.FORBUF_GET_FRONTLINE: endPointBuffer = FlToBuf.GetFrontLineBuffer(true); if (endPointBuffer == null) { if (countFrontLineNull++ < 10) { break; } countFrontLineNull = 0; robot.bayId = -1; robot.ShowText("Error Data Request FrontLine ForkLift" + order.dataRequest); order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE; TrafficRountineConstants.ReleaseAll(robot); robot.orderItem = null; robot.SwitchToDetectLine(false); robot.ReleaseWorkingZone(); if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) { TrafficRountineConstants.RegIntZone_READY.Release(robot); robot.robotTag = RobotStatus.IDLE; robot.SetSafeYellowcircle(false); robot.SetSafeBluecircle(false); robot.SetSafeSmallcircle(false); robot.TurnOnSupervisorTraffic(false); // rb.mcuCtrl.lampRbOff(); procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY; } else { procedureCode = ProcedureCode.PROC_CODE_FORKLIFT_TO_BUFFER; } ProRun = false; robot.ShowText("RELEASED FL---"); UpdateInformationInProc(this, ProcessStatus.S); order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; SaveOrderItem(order); KillEvent(); ReleaseProcedureHandler(this); } else { order.frontLinePos = endPointBuffer.Position; robot.bayId = bayId; StateForkLift = ForkLift.FORBUF_SELECT_BEHAVIOR_ONZONE; } break; case ForkLift.FORBUF_SELECT_BEHAVIOR_ONZONE: order.status = StatusOrderResponseCode.GOING_AND_PICKING_UP; if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) { if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY) { //if (false == rb.CheckInGateFromReadyZoneBehavior(doorServiceCtrl.PointFrontLine.Position)) { robot.ShowText("FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY"); registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = doorServiceCtrl.PointFrontLine.Position; StateForkLift = ForkLift.FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY; } } } else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position)) { robot.robotTag = RobotStatus.WORKING; if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine)) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG; // Cap Nhat Thong Tin CHuyen Di registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = doorServiceCtrl.PointFrontLine.Position; robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE"); } } else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position)) { // public void Start (ForkLiftToBuffer state = ForkLiftToBuffer.FORBUF_ROBOT_RELEASED) { robot.robotTag = RobotStatus.WORKING; if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine)) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = doorServiceCtrl.PointFrontLine.Position; robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_CHECKIN_GATE"); } } break; case ForkLift.FORBUF_ROBOT_GOTO_BACK_FRONTLINE_READY: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } robot.ShowText("Detetect Inside Station Check Ready"); if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_LEFT)) { Stopwatch sw = new Stopwatch(); sw.Start(); do { robot.onFlagGoBackReady = true; if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE || Traffic.RobotIsInArea("READY_FRONTLINE", robot.properties.pose.Position)) { robot.onFlagGoBackReady = false; robot.robotTag = RobotStatus.WORKING; if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine)) { resCmd = ResponseCommand.RESPONSE_NONE; StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY; robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE"); break; } } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } Thread.Sleep(100); } while (ProRunStopW); sw.Stop(); } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY: // dò ra điểm đích đến và xóa đăng ký vùng // TrafficRountineConstants.DetectRelease(registryRobotJourney); if (Traffic.RobotIsInArea("C3", rb.properties.pose.Position)) { ds.setDoorBusy(true); ds.openDoor(DoorService.DoorType.DOOR_BACK); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY_OPEN_DOOR; } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_READY_OPEN_DOOR: // dò ra điểm đích đến và xóa đăng ký vùng TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.setTrafficAllCircles(false, false, false, false); TrafficRountineConstants.RegIntZone_READY.Release(robot); robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION; robot.ShowText("FORBUF_ROBOT_CAME_GATE_POSITION"); } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG: // kiem tra vung đăng ký tai khu vuc xac định if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { // dò ra điểm đích đến và xóa đăng ký vùng StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM; } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM: // kiem tra vung đăng ký tai khu vuc xac định //TrafficRountineConstants.DetectRelease(registryRobotJourney); if (Traffic.RobotIsInArea("C3", rb.properties.pose.Position)) { ds.setDoorBusy(true); ds.openDoor(DoorService.DoorType.DOOR_BACK); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR; } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR: // kiem tra vung đăng ký tai khu vuc xac định TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { // robot.setTrafficAllCircles(false, false, false, false); TrafficRountineConstants.RegIntZone_READY.Release(robot); robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION; robot.ShowText("FORBUF_ROBOT_CAME_GATE_POSITION"); } break; case ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION: // da den khu vuc cong , gui yeu cau mo cong. robot.robotRegistryToWorkingZone.onRobotwillCheckInsideGate = false; //ds.setDoorBusy(true); //ds.openDoor(DoorService.DoorType.DOOR_BACK); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_OPEN_DOOR; robot.ShowText("FORBUF_ROBOT_WAITTING_OPEN_DOOR"); break; case ForkLift.FORBUF_ROBOT_WAITTING_OPEN_DOOR: //doi mo cong RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK); if (RetState.DOOR_CTRL_SUCCESS == ret || robot.onForceGoToGate) { ds.resetWaitOpenBack(); robot.onForceGoToGate = false; StateForkLift = ForkLift.FORBUF_ROBOT_OPEN_DOOR_SUCCESS; robot.ShowText("FORBUF_ROBOT_OPEN_DOOR_SUCCESS"); } else if (RetState.DOOR_CTRL_ERROR == ret) { robot.ShowText("FORBUF_ROBOT_OPEN_DOOR_ERROR"); //StateForkLift = ForkLift.FORBUF_ROBOT_CAME_GATE_POSITION; Thread.Sleep(1000); ds.setDoorBusy(true); ds.openDoor(DoorService.DoorType.DOOR_BACK); } break; case ForkLift.FORBUF_ROBOT_OPEN_DOOR_SUCCESS: // mo cua thang cong ,gui toa do line de robot di vao gap hang // rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETUP); if (rb.SendCmdAreaPallet(doorServiceCtrl.infoPallet)) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN; robot.ShowText("FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN"); } break; case ForkLift.FORBUF_ROBOT_WAITTING_PICKUP_PALLET_IN: // doi robot gap hang try { DoorStatus ret1 = ds.getStatusDoor(DoorType.DOOR_BACK); if (DoorStatus.DOOR_OPEN == ret1) { robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false); if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; StateForkLift = ForkLift.FORBUF_ROBOT_FINISH_PALLET_UP; Console.WriteLine("pallet ID" + order.palletId); robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.GOING_OUTSIDE_GATE; robot.ShowText("FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } //else if (DoorStatus.DOOR_UNKNOW == ret1) //{ //} else /* if(DoorStatus.DOOR_CLOSE == ret1)*/ { robot.ShowText("DOOR_ERROR_(-___-)"); robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_STOP, true); ds.setDoorBusy(true); ds.openDoor(DoorType.DOOR_BACK); Thread.Sleep(7000); } } catch { } break; case ForkLift.FORBUF_ROBOT_FINISH_PALLET_UP: String destName = Traffic.DetermineArea(endPointBuffer.Position, TypeZone.MAIN_ZONE); if (destName.Equals("VIM")) { if (checkAnyRobotAtElevator(robot)) { break; } else { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE; } } else { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE; } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE: // kiem tra da toi vung dong cong if (!Traffic.RobotIsInArea("CLOSE-GATE", robot.properties.pose.Position)) { DoorStatus ret2 = ds.getStatusDoor(DoorType.DOOR_BACK); if (DoorStatus.DOOR_OPEN == ret2) { robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false); if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { Global_Object.setGateStatus(order.gate, false); resCmd = ResponseCommand.RESPONSE_NONE; ds.LampSetStateOff(DoorType.DOOR_FRONT); ds.closeDoor(DoorType.DOOR_BACK); ds.setDoorBusy(false); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE; robot.ShowText("FORBUF_ROBOT_WAITTING_CLOSE_GATE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } //else if (DoorStatus.DOOR_UNKNOW == ret1) //{ //} else /* if(DoorStatus.DOOR_CLOSE == ret1)*/ { robot.ShowText("DOOR_ERROR_(-___-)"); robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_STOP, true); ds.setDoorBusy(true); ds.openDoor(DoorType.DOOR_BACK); Thread.Sleep(6000); } } else { robot.SetSpeedTraffic(RobotSpeedLevel.ROBOT_SPEED_NORMAL, false); if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { Global_Object.setGateStatus(order.gate, false); resCmd = ResponseCommand.RESPONSE_NONE; ds.LampSetStateOff(DoorType.DOOR_FRONT); ds.closeDoor(DoorType.DOOR_BACK); ds.setDoorBusy(false); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE; robot.ShowText("FORBUF_ROBOT_WAITTING_CLOSE_GATE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } break; case ForkLift.FORBUF_ROBOT_WAITTING_CLOSE_GATE: try { StateForkLift = ForkLift.FORBUF_ROBOT_CHECK_GOTO_BUFFER_OR_MACHINE; robot.SwitchToDetectLine(false); registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = endPointBuffer.Position; } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORBUF_ROBOT_CHECK_GOTO_BUFFER_OR_MACHINE: order.status = StatusOrderResponseCode.DELIVERING; flToMachineInfo = GetPriorityTaskForkLiftToMachine(order.productDetailId); if (flToMachineInfo == null) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_ZONE_BUFFER_READY; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = endPointBuffer.Position; } else { FreePlanedBuffer(order.palletId_P); UpdatePalletState(PalletStatus.W, flToMachineInfo.palletId, flToMachineInfo.planId); StateForkLift = ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM_REG; } break; case ForkLift.FORBUF_ROBOT_WAITTING_ZONE_BUFFER_READY: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } Pose destPos1 = endPointBuffer; if (rb.SendPoseStamped(destPos1)) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; robot.ShowText("FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } break; case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM; } break; case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM: TrafficRountineConstants.DetectRelease(registryRobotJourney); try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateForkLift = ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORBUF_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER: // xóa đăng ký vùng TrafficRountineConstants.DetectRelease(registryRobotJourney); if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; StateForkLift = ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORBUF_ROBOT_SEND_CMD_DROPDOWN_PALLET_BUFFER: JResult = FlToBuf.GetInfoPallet_P_InBuffer(PistonPalletCtrl.PISTON_PALLET_DOWN); String data = JsonConvert.SerializeObject(JResult.jInfoPallet); if (rb.SendCmdAreaPallet(data)) { StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER; robot.ShowText("FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER"); } break; case ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN) { resCmd = ResponseCommand.RESPONSE_NONE; FlToBuf.UpdatePalletState(PalletStatus.W, JResult.palletId, order.planId); // rb.SendCmdPosPallet (RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE); StateForkLift = ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER; robot.ShowText("FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { Console.WriteLine("Loi Update :ForkLift.FORBUF_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER"); errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER: // đợi if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.bayId = -1; robot.bayIdReg = false; robot.ReleaseWorkingZone(); robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; StateForkLift = ForkLift.FORBUF_ROBOT_RELEASED; robot.ShowText("FORBUF_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { Console.WriteLine("Loi Update :ForkLift.FORBUF_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER"); errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case ForkLift.FORBUF_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới rb.PreProcedureAs = ProcedureControlAssign.PRO_FORKLIFT_TO_BUFFER; ReleaseProcedureHandler(this); ProRun = false; robot.ShowText("RELEASED"); UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.FINISHED; break; case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM_REG: if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } else { StateForkLift = ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM; } break; case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE_FROM_VIM: TrafficRountineConstants.DetectRelease(registryRobotJourney); try { if (rb.SendPoseStamped(flToMachineInfo.frontLinePose)) { StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE; robot.ShowText("FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); onFlagResetedGate = false; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORMAC_ROBOT_GOTO_FRONTLINE_MACHINE: try { if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney)) { break; } if (rb.SendPoseStamped(flToMachineInfo.frontLinePose)) { StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE; robot.ShowText("FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE"); onFlagResetedGate = false; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORMAC_ROBOT_WAITTING_CAME_FRONTLINE_MACHINE: try { TrafficRountineConstants.DetectRelease(registryRobotJourney); if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); StateForkLift = ForkLift.FORMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET_MACHINE; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case ForkLift.FORMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET_MACHINE: if (rb.SendCmdAreaPallet(flToMachineInfo.infoPallet)) { StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE; robot.ShowText("FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE"); } break; case ForkLift.FORMAC_ROBOT_WAITTING_DROPDOWN_PALLET_MACHINE: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN) { resCmd = ResponseCommand.RESPONSE_NONE; // FlToBuf.UpdatePalletState(PalletStatus.W); // rb.SendCmdPosPallet (RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE); StateForkLift = ForkLift.FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE; robot.ShowText("FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case ForkLift.FORMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_MACHINE: // đợi if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; StateForkLift = ForkLift.FORMAC_ROBOT_RELEASED; robot.ShowText("FORMAC_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case ForkLift.FORMAC_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới ds.removeListCtrlDoorBack(); TrafficRountineConstants.ReleaseAll(robot); robot.orderItem = null; // Global_Object.onFlagDoorBusy = false; robot.SwitchToDetectLine(false); // robot.robotTag = RobotStatus.IDLE; robot.ReleaseWorkingZone(); rb.PreProcedureAs = ProcedureControlAssign.PRO_FORKLIFT_TO_BUFFER; // if (errorCode == ErrorCode.RUN_OK) { // } else { // ErrorProcedureHandler (this); // } ProRun = false; robot.ShowText("RELEASED"); UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.FINISHED; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); ReleaseProcedureHandler(this); break; case ForkLift.FORMAC_ROBOT_DESTROY: // trả robot về robotmanagement để nhận quy trình mới ds.removeListCtrlDoorBack(); TrafficRountineConstants.ReleaseAll(robot); robot.SwitchToDetectLine(false); robot.ReleaseWorkingZone(); robot.robotBahaviorAtGate = RobotBahaviorAtReadyGate.IDLE; //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; selectHandleError = SelectHandleError.CASE_ERROR_EXIT; procedureStatus = ProcedureStatus.PROC_KILLED; // RestoreOrderItem(); //FreePlanedBuffer(order.palletId_P); order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; KillEvent(); break; ////////////////////////////////////////////////////// default: break; } Thread.Sleep(700); } StateForkLift = ForkLift.FORBUF_IDLE; }
public void Procedure(object ojb) { ProcedureBufferToMachine BfToMa = (ProcedureBufferToMachine)ojb; RobotUnity rb = BfToMa.robot; // DataBufferToMachine p = BfToMa.points; TrafficManagementService Traffic = BfToMa.Traffic; robot.ShowText(" Start -> " + procedureCode); //GetFrontLineBuffer(false); // ProRun = false; rb.mcuCtrl.lampRbOn(); /* frontLinePose = BfToMa.GetFrontLineBuffer(); * if (frontLinePose == null) * { * robot.bayId = -1; * TrafficRountineConstants.ReleaseAll(robot); * robot.bayId = -1; * robot.bayIdReg = false; * robot.orderItem = null; * robot.SwitchToDetectLine(false); * if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) * { * TrafficRountineConstants.RegIntZone_READY.Release(robot); * robot.robotTag = RobotStatus.IDLE; * robot.SetSafeYellowcircle(false); * robot.SetSafeBluecircle(false); * robot.SetSafeSmallcircle(false); * robot.TurnOnSupervisorTraffic(false); * // rb.mcuCtrl.lampRbOff(); * procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY; * } * else * procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE; * ReleaseProcedureHandler(this); * ProRun = false; * UpdateInformationInProc(this, ProcessStatus.S); * order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE; * order.endTimeProcedure = DateTime.Now; * order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; * SaveOrderItem(order); * KillEvent(); * } * else * { * order.frontLinePos = frontLinePose.Position; * robot.bayId = bayId; * }*/ while (ProRun) { switch (StateBufferToMachine) { case BufferToMachine.BUFMAC_IDLE: //robot.ShowText("BUFMAC_IDLE"); break; case BufferToMachine.BUFMAC_GET_FRONTLINE: frontLinePose = BfToMa.GetFrontLineBuffer(); if (frontLinePose == null) { if (countFrontLineNull++ < 10) { break; } countFrontLineNull = 0; robot.bayId = -1; TrafficRountineConstants.ReleaseAll(robot); robot.bayId = -1; robot.bayIdReg = false; robot.orderItem = null; robot.SwitchToDetectLine(false); if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position)) { TrafficRountineConstants.RegIntZone_READY.Release(robot); robot.robotTag = RobotStatus.IDLE; robot.SetSafeYellowcircle(false); robot.SetSafeBluecircle(false); robot.SetSafeSmallcircle(false); robot.TurnOnSupervisorTraffic(false); // rb.mcuCtrl.lampRbOff(); procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY; } else { procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE; } ReleaseProcedureHandler(this); ProRun = false; UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; SaveOrderItem(order); KillEvent(); } else { order.frontLinePos = frontLinePose.Position; robot.bayId = bayId; StateBufferToMachine = BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE; } //robot.ShowText("BUFMAC_IDLE"); break; case BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE: if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS)) { if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY; //robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY"); //robot.ShowText("CHECK - REG"); } } else if (Traffic.RobotIsInArea("GATE", robot.properties.pose.Position, TypeZone.MAIN_ZONE)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; } else //if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position,TypeZone.MAIN_ZONE)) { Point destPos1 = frontLinePose.Position; String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE); if (rb.SendPoseStamped(frontLinePose)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS); registryRobotJourney.startPoint = robot.properties.pose.Position; registryRobotJourney.endPoint = destPos1; //robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER"); } } break; case BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY: if (Traffic.HasOtherRobotUnityinArea("GATE", robot)) { break; } if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT)) { Stopwatch sw = new Stopwatch(); sw.Start(); do { robot.onFlagGoBackReady = true; if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.onFlagGoBackReady = false; Point destPos2 = frontLinePose.Position; String destName2 = Traffic.DetermineArea(destPos2, TypeZone.MAIN_ZONE); //robot.ShowText("GO FRONTLINE IN OUTER"); if (rb.SendPoseStamped(frontLinePose)) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER; } break; } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); break; } Thread.Sleep(100); } while (ProRunStopW); sw.Stop(); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER: // nếu chưa đến goal phải check tới vùng c1 // nếu goal nằm trong C1 thì không đăng ký try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER; } else { if (!Traffic.PositionIsInArea(frontLinePose.Position).Equals("C1")) { if (TrafficRountineConstants.DetetectInsideStationCheckMerz(registryRobotJourney)) { break; } } if (TrafficCheckInBuffer(goalFrontLinePos, bayId)) { break; } } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER: JPallet jPallet_H = BfToMa.GetInfoPallet_H_InBuffer(PistonPalletCtrl.PISTON_PALLET_UP); jPResult = BfToMa.GetInfoOfPalletBuffer_Compare_W_H(PistonPalletCtrl.PISTON_PALLET_UP, jPallet_H); String data = JsonConvert.SerializeObject(jPResult.jInfoPallet); if (rb.SendCmdAreaPallet(data)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER; //robot.ShowText("BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER"); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP) { resCmd = ResponseCommand.RESPONSE_NONE; BfToMa.UpdatePalletState(PalletStatus.F, jPResult.palletId, order.planId); //Reset trạng thai W cho pallet_H nếu truoc đó có cây W if (jPResult.jInfoPallet.row < order.palletRow) { BfToMa.UpdatePalletState(PalletStatus.W, order.palletId_H, order.planId); } onUpdatedPalletState = true; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER; //robot.ShowText("BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER: // đợi try { if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.bayId = -1; robot.bayIdReg = false; robot.ReleaseWorkingZone(); robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE; // cập nhật lại điểm xuất phát registryRobotJourney.startPoint = robot.properties.pose.Position; //robot.ShowText("BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE: String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE); Pose destPos = BfToMa.GetFrontLineMachine(); String destName = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE); if (rb.SendPoseStamped(destPos)) { StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET; } break; case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET: // tai may try { if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT) { robot.SwitchToDetectLine(true); StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET; } } catch (System.Exception) { errorCode = ErrorCode.CAN_NOT_GET_DATA; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET: if (rb.SendCmdAreaPallet(BfToMa.GetInfoOfPalletMachine(PistonPalletCtrl.PISTON_PALLET_DOWN))) { //rb.prioritLevel.OnAuthorizedPriorityProcedure = true; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET; //robot.ShowText("BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET"); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET: if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN) { resCmd = ResponseCommand.RESPONSE_NONE; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE; //robot.ShowText("BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE: if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE) { robot.SwitchToDetectLine(false); resCmd = ResponseCommand.RESPONSE_NONE; //rb.prioritLevel.OnAuthorizedPriorityProcedure = false; StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED; //robot.ShowText("BUFMAC_ROBOT_RELEASED"); } else if (resCmd == ResponseCommand.RESPONSE_ERROR) { errorCode = ErrorCode.DETECT_LINE_ERROR; CheckUserHandleError(this); } break; case BufferToMachine.BUFMAC_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới TrafficRountineConstants.ReleaseAll(robot); robot.bayId = -1; robot.bayIdReg = false; robot.orderItem = null; robot.SwitchToDetectLine(false); // Release WorkinZone Robot // robot.robotTag = RobotStatus.IDLE; rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_MACHINE; // if (errorCode == ErrorCode.RUN_OK) { ReleaseProcedureHandler(this); // } else { // ErrorProcedureHandler (this); // } ProRun = false; //robot.ShowText("RELEASED"); UpdateInformationInProc(this, ProcessStatus.S); order.status = StatusOrderResponseCode.FINISHED; order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; SaveOrderItem(order); KillEvent(); break; case BufferToMachine.BUFMAC_ROBOT_DESTROY: order.endTimeProcedure = DateTime.Now; order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes; SaveOrderItem(order); robot.SwitchToDetectLine(false); robot.ReleaseWorkingZone(); // StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED; //robot.prioritLevel.OnAuthorizedPriorityProcedure = false; ProRun = false; UpdateInformationInProc(this, ProcessStatus.F); order.status = StatusOrderResponseCode.ROBOT_ERROR; //reset status pallet Faile H->Ws selectHandleError = SelectHandleError.CASE_ERROR_EXIT; procedureStatus = ProcedureStatus.PROC_KILLED; //FreeHoldBuffer(order.palletId_H); KillEvent(); //this.robot.DestroyRegistrySolvedForm(); break; default: break; } // robot.ShowText("-> " + procedureCode); Thread.Sleep(700); } StateBufferToMachine = BufferToMachine.BUFMAC_IDLE; }