Ejemplo n.º 1
0
        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;
        }