public ProcedureRobotToCharger(RobotUnity robot, ChargerManagementService charger, ChargerId id) : base(robot)
{
StateRobotToCharge = RobotGoToCharge.ROBCHAR_IDLE;
batLevel = new DataReceive();
statusCharger = new DataReceive();
this.robot = robot;
chargerCtrl = charger.ChargerStationList[id];
procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_CHARGE;
}
public void Start(RobotGoToCharge state = RobotGoToCharge.ROBCHAR_WAITTING_CHARGEBATTERY)
#endif
{
order = new OrderItem();
order.typeReq = TyeRequest.TYPEREQUEST_CHARGE;
robot.robotTag = RobotStatus.CHARGING;
errorCode = ErrorCode.RUN_OK;
robot.ProcedureAs = ProcedureControlAssign.PRO_CHARGE;
StateRobotToCharge = state;
ProRobotToCharger = new Thread(this.Procedure);
procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_CHARGE;
ProRun = true;
//robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
ProRobotToCharger.Start(this);
}
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 Start(RobotGoToCharge state = RobotGoToCharge.ROBCHAR_ROBOT_GOTO_CHARGER)
