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