Example #1
0
        /// <summary>
        /// Update state after recieving return data from robot
        /// </summary>
        /// <param name="response">response from scribbler robot</param>
        private IEnumerator <ITask> ScribblerResponseHandler(ScribblerResponseMessage response) //private void UpdateState(ScribblerResponse response) //debug
        {
            //initialize notify list
            List <string> notify = new List <string>();

            switch ((ScribblerHelper.Commands)response.Body.CommandType)
            {
            case ScribblerHelper.Commands.GET_STATE:
                ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Body.Data[0], response.Body.Data[1]);
                if (_state.Stall != parse_get_state.Stall)
                {
                    notify.Add("STALL");
                    _state.Stall = parse_get_state.Stall;
                }
                if (_state.LineLeft != parse_get_state.LineLeft)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = parse_get_state.LineLeft;
                }
                if (_state.LineRight != parse_get_state.LineRight)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = parse_get_state.LineRight;
                }
                _state.LEDLeft   = parse_get_state.LedLeft;
                _state.LEDCenter = parse_get_state.LedCenter;
                _state.LEDRight  = parse_get_state.LedRight;
                break;

            case ScribblerHelper.Commands.GET_IR_LEFT:
                bool New_Get_Open_Left = response.Body.Data[0] == 1;                 //NOTE: Not inverting logic here
                if (_state.IRLeft != New_Get_Open_Left)
                {
                    notify.Add("IRLEFT");
                    _state.IRLeft = New_Get_Open_Left;
                }
                break;

            case ScribblerHelper.Commands.GET_IR_RIGHT:
                bool New_Get_Open_Right = response.Body.Data[0] == 1;                 //NOTE: Not inverting logic here
                if (_state.IRRight != New_Get_Open_Right)
                {
                    notify.Add("IRRIGHT");
                    _state.IRRight = New_Get_Open_Right;
                }
                break;

            case ScribblerHelper.Commands.GET_STALL:
                bool New_Get_Stall = response.Body.Data[0] == 1;
                if (_state.Stall != New_Get_Stall)
                {
                    notify.Add("STALL");
                    _state.Stall = New_Get_Stall;
                }
                break;

            case ScribblerHelper.Commands.GET_LIGHT_LEFT:
                _state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                notify.Add("LIGHTLEFT");
                break;

            case ScribblerHelper.Commands.GET_LIGHT_CENTER:
                _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                notify.Add("LIGHTCENTER");
                break;

            case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
                _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                notify.Add("LIGHTRIGHT");
                break;

            case ScribblerHelper.Commands.GET_LINE_RIGHT:
                bool New_Get_Line_Right = response.Body.Data[0] == 1;
                if (_state.LineRight != New_Get_Line_Right)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = New_Get_Line_Right;
                }
                break;

            case ScribblerHelper.Commands.GET_LINE_LEFT:
                bool New_Get_Line_Left = response.Body.Data[0] == 1;
                if (_state.LineLeft != New_Get_Line_Left)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = New_Get_Line_Left;
                }
                break;

            case ScribblerHelper.Commands.GET_NAME:
                Encoding.ASCII.GetChars(response.Body.Data, 0, 8);
                break;

            case ScribblerHelper.Commands.GET_LIGHT_ALL:
                //TODO: this can be simplified if the scribbler sent low byte first
                _state.LightLeft               = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                _state.LightCenter             = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
                _state.LightRight              = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
                _state.LightSensorsLastUpdated = DateTime.Now;
                notify.Add("LIGHTLEFT");
                notify.Add("LIGHTCENTER");
                notify.Add("LIGHTRIGHT");
                break;

            case ScribblerHelper.Commands.GET_IR_ALL:
                bool newirleft  = response.Body.Data[0] == 1;       //NOTE: Not inverting logic here
                bool newirright = response.Body.Data[1] == 1;
                if (_state.IRLeft != newirleft)
                {
                    notify.Add("IRLEFT");
                    _state.IRLeft = newirleft;
                }
                if (_state.IRRight != newirright)
                {
                    notify.Add("IRRIGHT");
                    _state.IRRight = newirright;
                }
                _state.LineSensorsLastUpdated = DateTime.Now;
                _state.BumpersLastUpdated     = DateTime.Now;
                break;

            case ScribblerHelper.Commands.GET_LINE_ALL:
                bool newlineleft  = response.Body.Data[0] == 1;
                bool newlineright = response.Body.Data[1] == 1;
                if (_state.LineLeft != newlineleft)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = newlineleft;
                }
                if (_state.LineRight != newlineright)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = newlineright;
                }
                _state.LineSensorsLastUpdated = DateTime.Now;
                break;

            case ScribblerHelper.Commands.GET_ALL:
            case ScribblerHelper.Commands.SET_MOTORS_OFF:
            case ScribblerHelper.Commands.SET_MOTORS:
            case ScribblerHelper.Commands.SET_LED_LEFT_ON:
            case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
            case ScribblerHelper.Commands.SET_LED_CENTER_ON:
            case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
            case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
            case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
            case ScribblerHelper.Commands.SET_SPEAKER:
            case ScribblerHelper.Commands.SET_SPEAKER_2:
            case ScribblerHelper.Commands.SET_NAME:
            case ScribblerHelper.Commands.SET_LED_ALL_ON:
            case ScribblerHelper.Commands.SET_LED_ALL_OFF:
            case ScribblerHelper.Commands.SET_LED_ALL:
            case ScribblerHelper.Commands.SET_LOUD:
            case ScribblerHelper.Commands.SET_QUIET:
            case ScribblerHelper.Commands.SET_DATA:
            case ScribblerHelper.Commands.SET_ECHO_MODE:
                bool New_Get_All_IR_Left  = response.Body.Data[0] == 1;       //NOTE: Not inverting logic here
                bool New_Get_All_IR_Right = response.Body.Data[1] == 1;
                if (_state.IRLeft != New_Get_All_IR_Left)
                {
                    notify.Add("IRLEFT");
                    _state.IRLeft = New_Get_All_IR_Left;
                }
                if (_state.IRRight != New_Get_All_IR_Right)
                {
                    notify.Add("IRRIGHT");
                    _state.IRRight = New_Get_All_IR_Right;
                }

                _state.LightLeft   = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
                _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
                _state.LightRight  = BitConverter.ToUInt16(new byte[] { response.Body.Data[7], response.Body.Data[6] }, 0);
                notify.Add("LIGHTLEFT");
                notify.Add("LIGHTCENTER");
                notify.Add("LIGHTRIGHT");

                bool New_Get_All_Line_Left  = response.Body.Data[8] == 1;
                bool New_Get_All_Line_Right = response.Body.Data[9] == 1;
                if (_state.LineLeft != New_Get_All_Line_Left)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = New_Get_All_Line_Left;
                }
                if (_state.LineRight != New_Get_All_Line_Right)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = New_Get_All_Line_Right;
                }

                bool newstall = response.Body.Data[10] == 1;
                if (_state.Stall != newstall)
                {
                    notify.Add("STALL");
                    _state.Stall = newstall;
                }
                _state.LineSensorsLastUpdated  = DateTime.Now;
                _state.BumpersLastUpdated      = DateTime.Now;
                _state.LightSensorsLastUpdated = DateTime.Now;
                break;

            case ScribblerHelper.Commands.GET_ALL_BINARY:
                ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Body.Data[0]);

                if (_state.IRLeft != parse_all_binary.IRLeft)
                {
                    notify.Add("IRLEFT");
                    _state.IRLeft = parse_all_binary.IRLeft;           //NOTE: Not inverting logic here
                }
                if (_state.IRRight != parse_all_binary.IRRight)
                {
                    notify.Add("IRRIGHT");
                    _state.IRRight = parse_all_binary.IRRight;
                }

                if (_state.LineLeft != parse_all_binary.LineLeft)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = parse_all_binary.LineLeft;
                }
                if (_state.LineRight != parse_all_binary.LineRight)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = parse_all_binary.LineRight;
                }

                if (_state.Stall != parse_all_binary.Stall)
                {
                    notify.Add("STALL");
                    _state.Stall = parse_all_binary.Stall;
                }
                _state.LineSensorsLastUpdated = DateTime.Now;
                _state.BumpersLastUpdated     = DateTime.Now;
                break;

            case ScribblerHelper.Commands.GET_INFO:
                Console.WriteLine(System.Text.Encoding.ASCII.GetChars(response.Body.Data));
                break;

            case ScribblerHelper.Commands.GET_DATA:
                foreach (byte b in response.Body.Data)
                {
                    Console.Write(b);
                }
                Console.Write("\n");
                break;

            default:
                LogError(LogGroups.Console, "Update State command missmatch, got " + response.Body.CommandType);
                break;
            }

            // notify general subscribers
            subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));

            // notify selective subscribers
            submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
            subMgrPort.Post(sub);

            yield break;
        }
Example #2
0
        /// <summary>
        /// Update state after recieving return data from robot
        /// </summary>
        /// <param name="response">response from scribbler robot</param>
        private IEnumerator<ITask> ScribblerResponseHandler(ScribblerResponseMessage response) //private void UpdateState(ScribblerResponse response) //debug
        {
            //initialize notify list
            List<string> notify = new List<string>();

            switch ((ScribblerHelper.Commands)response.Body.CommandType)
            {
                case ScribblerHelper.Commands.GET_STATE:
                    ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Body.Data[0], response.Body.Data[1]);
                    if (_state.Stall != parse_get_state.Stall)
                    {
                        notify.Add("STALL");
                        _state.Stall = parse_get_state.Stall;
                    }
                    if (_state.LineLeft != parse_get_state.LineLeft)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = parse_get_state.LineLeft;
                    }
                    if (_state.LineRight != parse_get_state.LineRight)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = parse_get_state.LineRight;
                    }
                    _state.LEDLeft = parse_get_state.LedLeft;
                    _state.LEDCenter = parse_get_state.LedCenter;
                    _state.LEDRight = parse_get_state.LedRight;
                    break;
                case ScribblerHelper.Commands.GET_IR_LEFT:
                    bool New_Get_Open_Left = response.Body.Data[0] == 1;             //NOTE: Not inverting logic here
                    if (_state.IRLeft != New_Get_Open_Left)
                    {
                        notify.Add("IRLEFT");
                        _state.IRLeft = New_Get_Open_Left;
                    }
                    break;
                case ScribblerHelper.Commands.GET_IR_RIGHT:
                    bool New_Get_Open_Right = response.Body.Data[0] == 1;             //NOTE: Not inverting logic here
                    if (_state.IRRight != New_Get_Open_Right)
                    {
                        notify.Add("IRRIGHT");
                        _state.IRRight = New_Get_Open_Right;
                    }
                    break;
                case ScribblerHelper.Commands.GET_STALL:
                    bool New_Get_Stall = response.Body.Data[0] == 1;
                    if (_state.Stall != New_Get_Stall)
                    {
                        notify.Add("STALL");
                        _state.Stall = New_Get_Stall;
                    }
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_LEFT:
                    _state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                    notify.Add("LIGHTLEFT");
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_CENTER:
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                    notify.Add("LIGHTCENTER");
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                    notify.Add("LIGHTRIGHT");
                    break;
                case ScribblerHelper.Commands.GET_LINE_RIGHT:
                    bool New_Get_Line_Right = response.Body.Data[0] == 1;
                    if (_state.LineRight != New_Get_Line_Right)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = New_Get_Line_Right;
                    }
                    break;
                case ScribblerHelper.Commands.GET_LINE_LEFT:
                    bool New_Get_Line_Left = response.Body.Data[0] == 1;
                    if (_state.LineLeft != New_Get_Line_Left)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = New_Get_Line_Left;
                    }
                    break;
                case ScribblerHelper.Commands.GET_NAME:
                    Encoding.ASCII.GetChars(response.Body.Data, 0, 8);
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_ALL:
                    //TODO: this can be simplified if the scribbler sent low byte first
                    _state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[1], response.Body.Data[0] }, 0);
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
                    _state.LightSensorsLastUpdated = DateTime.Now;
                    notify.Add("LIGHTLEFT");
                    notify.Add("LIGHTCENTER");
                    notify.Add("LIGHTRIGHT");
                    break;
                case ScribblerHelper.Commands.GET_IR_ALL:
                    bool newirleft = response.Body.Data[0] == 1;    //NOTE: Not inverting logic here
                    bool newirright = response.Body.Data[1] == 1;
                    if (_state.IRLeft != newirleft)
                    {
                        notify.Add("IRLEFT");
                        _state.IRLeft = newirleft;
                    }
                    if (_state.IRRight != newirright)
                    {
                        notify.Add("IRRIGHT");
                        _state.IRRight = newirright;
                    }
                    _state.LineSensorsLastUpdated = DateTime.Now;
                    _state.BumpersLastUpdated = DateTime.Now;
                    break;
                case ScribblerHelper.Commands.GET_LINE_ALL:
                    bool newlineleft = response.Body.Data[0] == 1;
                    bool newlineright = response.Body.Data[1] == 1;
                    if (_state.LineLeft != newlineleft)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = newlineleft;
                    }
                    if (_state.LineRight != newlineright)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = newlineright;
                    }
                    _state.LineSensorsLastUpdated = DateTime.Now;
                    break;
                case ScribblerHelper.Commands.GET_ALL:
                case ScribblerHelper.Commands.SET_MOTORS_OFF:
                case ScribblerHelper.Commands.SET_MOTORS:
                case ScribblerHelper.Commands.SET_LED_LEFT_ON:
                case ScribblerHelper.Commands.SET_LED_LEFT_OFF:
                case ScribblerHelper.Commands.SET_LED_CENTER_ON:
                case ScribblerHelper.Commands.SET_LED_CENTER_OFF:
                case ScribblerHelper.Commands.SET_LED_RIGHT_ON:
                case ScribblerHelper.Commands.SET_LED_RIGHT_OFF:
                case ScribblerHelper.Commands.SET_SPEAKER:
                case ScribblerHelper.Commands.SET_SPEAKER_2:
                case ScribblerHelper.Commands.SET_NAME:
                case ScribblerHelper.Commands.SET_LED_ALL_ON:
                case ScribblerHelper.Commands.SET_LED_ALL_OFF:
                case ScribblerHelper.Commands.SET_LED_ALL:
                case ScribblerHelper.Commands.SET_LOUD:
                case ScribblerHelper.Commands.SET_QUIET:
                case ScribblerHelper.Commands.SET_DATA:
                case ScribblerHelper.Commands.SET_ECHO_MODE:
                    bool New_Get_All_IR_Left = response.Body.Data[0] == 1;    //NOTE: Not inverting logic here
                    bool New_Get_All_IR_Right = response.Body.Data[1] == 1;
                    if (_state.IRLeft != New_Get_All_IR_Left)
                    {
                        notify.Add("IRLEFT");
                        _state.IRLeft = New_Get_All_IR_Left;
                    }
                    if (_state.IRRight != New_Get_All_IR_Right)
                    {
                        notify.Add("IRRIGHT");
                        _state.IRRight = New_Get_All_IR_Right;
                    }

                    _state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Body.Data[3], response.Body.Data[2] }, 0);
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Body.Data[5], response.Body.Data[4] }, 0);
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Body.Data[7], response.Body.Data[6] }, 0);
                    notify.Add("LIGHTLEFT");
                    notify.Add("LIGHTCENTER");
                    notify.Add("LIGHTRIGHT");

                    bool New_Get_All_Line_Left = response.Body.Data[8] == 1;
                    bool New_Get_All_Line_Right = response.Body.Data[9] == 1;
                    if (_state.LineLeft != New_Get_All_Line_Left)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = New_Get_All_Line_Left;
                    }
                    if (_state.LineRight != New_Get_All_Line_Right)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = New_Get_All_Line_Right;
                    }

                    bool newstall = response.Body.Data[10] == 1;
                    if (_state.Stall != newstall)
                    {
                        notify.Add("STALL");
                        _state.Stall = newstall;
                    }
                    _state.LineSensorsLastUpdated = DateTime.Now;
                    _state.BumpersLastUpdated = DateTime.Now;
                    _state.LightSensorsLastUpdated = DateTime.Now;
                    break;
                case ScribblerHelper.Commands.GET_ALL_BINARY:
                    ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.Body.Data[0]);

                    if (_state.IRLeft != parse_all_binary.IRLeft)
                    {
                        notify.Add("IRLEFT");
                        _state.IRLeft = parse_all_binary.IRLeft;       //NOTE: Not inverting logic here
                    }
                    if (_state.IRRight != parse_all_binary.IRRight)
                    {
                        notify.Add("IRRIGHT");
                        _state.IRRight = parse_all_binary.IRRight;
                    }

                    if (_state.LineLeft != parse_all_binary.LineLeft)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = parse_all_binary.LineLeft;
                    }
                    if (_state.LineRight != parse_all_binary.LineRight)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = parse_all_binary.LineRight;
                    }

                    if (_state.Stall != parse_all_binary.Stall)
                    {
                        notify.Add("STALL");
                        _state.Stall = parse_all_binary.Stall;
                    }
                    _state.LineSensorsLastUpdated = DateTime.Now;
                    _state.BumpersLastUpdated = DateTime.Now;
                    break;
                case ScribblerHelper.Commands.GET_INFO:
                    Console.WriteLine(System.Text.Encoding.ASCII.GetChars(response.Body.Data));
                    break;
                case ScribblerHelper.Commands.GET_DATA:
                    foreach (byte b in response.Body.Data)
                        Console.Write(b);
                    Console.Write("\n");
                    break;
                case 0:
                    //Console.WriteLine("Got 0 command");
                    // Do nothing
                    break;
                default:
                    LogError(LogGroups.Console, "Update State command missmatch, got " + response.Body.CommandType);
                    break;
            }

            // notify general subscribers
            subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));

            // notify selective subscribers
            submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray());
            subMgrPort.Post(sub);

            yield break;
        }