// public override event Action<Object> ErrorProcedureHandler;
public ProcedureBufferToMachine(RobotUnity robot, TrafficManagementService trafficService) : base(robot, trafficService)
{
StateBufferToMachine = BufferToMachine.BUFMAC_IDLE;
this.robot = robot;
// this.points = new DataBufferToMachine();
this.Traffic = trafficService;
procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE;
}
public void Destroy()
{
ProRunStopW = false;
robot.robotTag = RobotStatus.IDLE;
robot.orderItem = null;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_DESTROY;
TrafficRountineConstants.ReleaseAll(robot);
robot.bayId = -1;
robot.bayIdReg = false;
}
public void Start(BufferToMachine state = BufferToMachine.BUFMAC_GET_FRONTLINE)
{
errorCode = ErrorCode.RUN_OK;
robot.robotTag = RobotStatus.WORKING;
robot.ProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_MACHINE;
StateBufferToMachine = state;
Task ProBuferToMachine = new Task(() => this.Procedure(this));
procedureStatus = ProcedureStatus.PROC_ALIVE;
ProRun = true;
ProRunStopW = true;
//robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
order.startTimeProcedure = DateTime.Now;
registryRobotJourney = new RegistryRobotJourney();
registryRobotJourney.robot = robot;
registryRobotJourney.traffic = Traffic;
robot.bayId = -1;
robot.bayIdReg = false;
ProBuferToMachine.Start();
}
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;
}
