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