Example #1
0
        /// <summary>
        /// Read Serial Port for echo
        /// </summary>
        /// <param name="outBuff">The outbound message to match</param>
        /// <returns>ScribblerResponse</returns>
        private ScribblerResponse GetEcho(byte[] outBuff)
        {
            byte[]            inBuff   = new byte[outMessageSize];
            ScribblerResponse response = null;
            int      ixOutBuff         = 0;
            DateTime lastbytetime      = DateTime.Now;

            try
            {
                while (ixOutBuff < outMessageSize) // && Compare(DateTime.Now, lastbytetime) < ReadTimeOut)
                {
                    byte[] temp = new byte[1];
                    _serialPort.Read(temp, 0, 1); //get 1 byte
                    if (temp[0] == outBuff[ixOutBuff])
                    {
                        inBuff[ixOutBuff] = temp[0];
                        ixOutBuff++;
                        lastbytetime = DateTime.Now;
                    }
                    else
                    {
                        Console.WriteLine("Echo missmatch");
                        break;
                    }
                }

                response      = new ScribblerResponse();
                response.Data = (byte[])inBuff.Clone();

                #if DEBUG
                Console.Write("Echo: ");
                foreach (byte b in response.Data)
                {
                    if (b != 0)
                    {
                        Console.Write(b + " ");
                    }
                    else
                    {
                        Console.Write("` ");
                    }
                }
                Console.Write("\n");
                #endif
            }
            catch (Exception ex)
            {
                Console.WriteLine("GetCommandResponse Exception: " + ex);
            }
            return(response);
        }
Example #2
0
        //private IEnumerator<ITask> ConfigureSensorHandler(ConfigureSensor command)
        //{
        //    if (command.Body == null)
        //    {
        //        command.ResponsePort.Post(new Fault());
        //        yield break;
        //    }

        //    if (command.Body.Sensor == null || command.Body.Configuration == null)
        //    {
        //        command.ResponsePort.Post(new Fault());
        //        yield break;
        //    }

        //    switch (command.Body.Sensor.ToUpper())
        //    {
        //        case "LEFT":
        //            _state.LightLeftConfig = command.Body.Configuration;
        //        break;

        //        case "CENTER":
        //            _state.LightCenterConfig = command.Body.Configuration;
        //        break;

        //        case "RIGHT":
        //            _state.LightRightConfig = command.Body.Configuration;
        //        break;
        //    }

        //    command.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        //    yield break;
        //}


        /// <summary>
        /// Send a command to the Scribbler and wait for a response.
        /// </summary>
        /// <param name="ready"></param>
        /// <param name="legoCommand"></param>
        /// <returns></returns>
        private IEnumerator <ITask> SendScribblerCommandHandler(SendScribblerCommand command)
        {
            // Send command to robot and wait for echo and response
            ScribblerResponse validResponse = _scribblerCom.SendCommand(command.Body);

            if (validResponse == null)
            {
                LogError(LogGroups.Console, "Send Scribbler Command null response");
                command.ResponsePort.Post(new Fault());
            }
            //else if (validResponse.GetType() == typeof(LegoResponseException))
            //{
            //    // Pull exception text from response
            //    string errorMessage = "LEGO command: " + command.Body.LegoCommandCode.ToString() + " response generated an error: " + ((LegoResponseException)validResponse).ErrorMessage;
            //    command.ResponsePort.Post(new SoapFaultContext(new InvalidOperationException(errorMessage)).SoapFault);
            //}
            else
            {
                // Check to see if we need to update state
                // based on the response received from LEGO.
                //    PortSet<DefaultUpdateResponseType, Fault> responsePort = UpdateCurrentState(validResponse);
                //    if (responsePort != null)
                //    {
                //        yield return Arbiter.Choice(responsePort,
                //            delegate(DefaultUpdateResponseType response) { },
                //            delegate(Fault fault)
                //            {
                //                LogError(LogGroups.Console, "Failed to update LEGO NXT service state", fault);
                //            });
                //    }

                //    PostCommandProcessing(validResponse);

                //reset timer
                PollTimer.Enabled = false;
                PollTimer.Enabled = true;

                //Update our state with the scribbler's response
                UpdateState(validResponse);

                command.ResponsePort.Post(validResponse);
            }

            // Ready to process another command
            Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
            yield break;
        }
Example #3
0
        /// <summary>
        /// Read Serial Port for echo
        /// </summary>
        /// <param name="outBuff">The outbound message to match</param>
        /// <returns>ScribblerResponse</returns>
        private ScribblerResponse GetEcho(byte[] outBuff)
        {
            byte[]            inBuff   = new byte[outMessageSize];
            ScribblerResponse response = null;
            int ixOutBuff = 0;

            try
            {
                while (ixOutBuff < outMessageSize)
                {
                    byte[] temp = new byte[1];
                    _serialPort.Read(temp, 0, 1); //get 1 byte
                    if (temp[0] == outBuff[ixOutBuff])
                    {
                        inBuff[ixOutBuff] = temp[0];
                        ixOutBuff++;
                    }
                }

                response      = new ScribblerResponse();
                response.Data = (byte[])inBuff.Clone();

                //DEBUG
                //Console.Write("Echo: ");
                //foreach (byte b in response.Data)
                //{
                //    if (b != 0)
                //        Console.Write(b + " ");
                //    else
                //        Console.Write("` ");
                //}
                //Console.Write("\n");
                //DEBUG
            }
            catch (Exception ex)
            {
                Console.WriteLine("GetCommandResponse Exception: " + ex);
            }
            return(response);
        }
Example #4
0
        /// <summary>
        /// Update state after recieving return data from robot
        /// </summary>
        /// <param name="response"></param>
        private void UpdateState(ScribblerResponse response)
        {
            //initialize notify list
            List <string> notify = new List <string>();

            switch ((ScribblerHelper.Commands)response.CommandType)
            {
            case ScribblerHelper.Commands.GET_STATE:
                ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.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_OPEN_LEFT:
                bool New_Get_Open_Left = response.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_OPEN_RIGHT:
                bool New_Get_Open_Right = response.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.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.Data[1], response.Data[0] }, 0);
                //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                notify.Add("LIGHTLEFT");
                break;

            case ScribblerHelper.Commands.GET_LIGHT_CENTER:
                _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                notify.Add("LIGHTCENTER");
                break;

            case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
                _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                notify.Add("LIGHTRIGHT");
                break;

            case ScribblerHelper.Commands.GET_LINE_RIGHT:
                bool New_Get_Line_Right = response.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.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.Data, 0, 8);
                break;

            case ScribblerHelper.Commands.GET_LIGHT_ALL:
                _state.LightLeft   = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
                _state.LightRight  = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
                //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                notify.Add("LIGHTLEFT");
                //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                notify.Add("LIGHTCENTER");
                //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                notify.Add("LIGHTRIGHT");
                break;

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

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

            case ScribblerHelper.Commands.GET_ALL:
                bool New_Get_All_IR_Left  = response.Data[0] == 1;       //NOTE: Not inverting logic here
                bool New_Get_All_IR_Right = response.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.Data[3], response.Data[2] }, 0);
                _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
                _state.LightRight  = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
                //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                notify.Add("LIGHTLEFT");
                //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                notify.Add("LIGHTCENTER");
                //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                notify.Add("LIGHTRIGHT");

                bool New_Get_All_Line_Left  = response.Data[8] == 1;
                bool New_Get_All_Line_Right = response.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.Data[10] == 1;
                if (_state.Stall != newstall)
                {
                    notify.Add("STALL");
                    _state.Stall = newstall;
                }
                break;

            case ScribblerHelper.Commands.GET_ALL_BINARY:
            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_LOUD:
            case ScribblerHelper.Commands.SET_QUIET:
                ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.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;
                }
                break;

            default:
                LogError(LogGroups.Console, "Update State command missmatch");
                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);
        }
Example #5
0
 public ScribblerResponseMessage(ScribblerResponse b)
 {
     base.Body = b;
 }
Example #6
0
        /// <summary>
        /// Read Serial Port for a number of bytes and put into ScribblerResponse
        /// </summary>
        /// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
        /// <returns>ScribblerResponse</returns>
        private ScribblerResponse GetCommandResponse(int nBytes)
        {
            byte[]            inBuff   = new byte[nBytes];
            ScribblerResponse response = null;
            int read = 0;

            try
            {
                while (read < nBytes)
                {
                    int canread;
                    while ((canread = _serialPort.BytesToRead) == 0)
                    {
                        ;                                              //spin
                    }
                    int needtoread = nBytes - read;
                    if (canread > needtoread)
                    {
                        _serialPort.Read(inBuff, read, needtoread);
                        read += needtoread;
                    }
                    else
                    {
                        _serialPort.Read(inBuff, read, canread);
                        read += canread;
                    }
                    //DEBUG
                    //foreach (byte b in inBuff)
                    //{
                    //    if (b != 0)
                    //        Console.Write(b + " ");
                    //    else
                    //        Console.Write("` ");
                    //}
                    //Console.Write("\n");
                    //DEBUG
                }

                response             = new ScribblerResponse(nBytes - 1);
                response.CommandType = inBuff[inBuff.Length - 1];
                for (int i = 0; i < inBuff.Length - 1; i++)
                {
                    response.Data[i] = inBuff[i];
                }

                //DEBUG
                //Console.Write("Got: ");
                //foreach (byte b in response.Data)
                //{
                //    if (b != 0)
                //        Console.Write(b + " ");
                //    else
                //        Console.Write("` ");
                //}
                //Console.Write("\n");
                //DEBUG
            }
            catch (Exception ex)
            {
                Console.WriteLine("GetCommandResponse Exception: " + ex);
            }
            return(response);
        }
Example #7
0
        /// <summary>
        /// DO NOT USE THIS COMMAND DIRECTLY.
        /// In Scribbler.cs, post a message to _scribblerComPort
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        internal ScribblerResponse SendCommand(ScribblerCommand cmd)
        {
            ScribblerResponse echo     = null;
            ScribblerResponse response = null;

            byte[] buffer = new byte[outMessageSize];

            if (buffer != null)
            {
                int ix = 0;

                //buffer = cmd.ToByteArray();

                buffer[ix++] = cmd.CommandType;

                if (cmd.Data != null && cmd.Data.Length > 0)
                {
                    foreach (byte b in cmd.Data)
                    {
                        buffer[ix++] = b;
                    }
                }

                //fill to standard size
                while (ix < outMessageSize)
                {
                    buffer[ix++] = 0;
                }

                //DEBUG
                //Console.Write("\nSent: ");
                //foreach (byte b in buffer)
                //{
                //    if (b != 0)
                //        Console.Write(b + " ");
                //    else
                //        Console.Write("` ");
                //}
                //Console.Write("\n");
                //DEBUG

                // When requesting a response, clear the inbound buffer
                if (_serialPort.BytesToRead > 0)
                {
                    _serialPort.DiscardInBuffer();
                }

                try
                {
                    _serialPort.Write(buffer, 0, ix);
                }
                catch
                {
                    Console.WriteLine("Serial Port Timeout.  Turn on Scribbler.");
                    //throw new IOException();
                }

                echo     = GetEcho(buffer);
                response = GetCommandResponse(helper.ReturnSize((ScribblerHelper.Commands)cmd.CommandType));
            }
            return(response);
        }
Example #8
0
        /// <summary>
        /// Update state after recieving return data from robot
        /// </summary>
        /// <param name="response"></param>
        private void UpdateState(ScribblerResponse response)
        {
            //initialize notify list
            List<string> notify = new List<string>();

            switch ((ScribblerHelper.Commands)response.CommandType)
            {
                case ScribblerHelper.Commands.GET_STATE:
                    ScribblerHelper.GetStatusDecomp parse_get_state = new ScribblerHelper.GetStatusDecomp(response.Data[0], response.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_OPEN_LEFT:
                    bool New_Get_Open_Left = response.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_OPEN_RIGHT:
                    bool New_Get_Open_Right = response.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.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.Data[1], response.Data[0] }, 0);
                    //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                        notify.Add("LIGHTLEFT");
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_CENTER:
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                    //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                        notify.Add("LIGHTCENTER");
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_RIGHT:
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                    //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                        notify.Add("LIGHTRIGHT");
                    break;
                case ScribblerHelper.Commands.GET_LINE_RIGHT:
                    bool New_Get_Line_Right = response.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.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.Data, 0, 8);
                    break;
                case ScribblerHelper.Commands.GET_LIGHT_ALL:
                    _state.LightLeft = BitConverter.ToUInt16(new byte[] { response.Data[1], response.Data[0] }, 0);
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[3], response.Data[2] }, 0);
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
                    //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                        notify.Add("LIGHTLEFT");
                    //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                        notify.Add("LIGHTCENTER");
                    //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                        notify.Add("LIGHTRIGHT");
                    break;
                case ScribblerHelper.Commands.GET_IR_ALL:
                    bool newirleft = response.Data[0] == 1;    //NOTE: Not inverting logic here
                    bool newirright = response.Data[1] == 1;
                    if (_state.IRLeft != newirleft)
                    {
                        notify.Add("IRLEFT");
                        _state.IRLeft = newirleft;
                    }
                    if (_state.IRRight != newirright)
                    {
                        notify.Add("IRRIGHT");
                        _state.IRRight = newirright;
                    }
                    break;
                case ScribblerHelper.Commands.GET_LINE_ALL:
                    bool newlineleft = response.Data[0] == 1;
                    bool newlineright = response.Data[1] == 1;
                    if (_state.LineLeft != newlineleft)
                    {
                        notify.Add("LINELEFT");
                        _state.LineLeft = newlineleft;
                    }
                    if (_state.LineRight != newlineright)
                    {
                        notify.Add("LINERIGHT");
                        _state.LineRight = newlineright;
                    }
                    break;
                case ScribblerHelper.Commands.GET_ALL:
                    bool New_Get_All_IR_Left = response.Data[0] == 1;    //NOTE: Not inverting logic here
                    bool New_Get_All_IR_Right = response.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.Data[3], response.Data[2] }, 0);
                    _state.LightCenter = BitConverter.ToUInt16(new byte[] { response.Data[5], response.Data[4] }, 0);
                    _state.LightRight = BitConverter.ToUInt16(new byte[] { response.Data[7], response.Data[6] }, 0);
                    //if (_state.LightLeftConfig.GreaterThan && (_state.LightLeft > _state.LightLeftConfig.Threshold))
                        notify.Add("LIGHTLEFT");
                    //if (_state.LightCenterConfig.GreaterThan && (_state.LightCenter > _state.LightCenterConfig.Threshold))
                        notify.Add("LIGHTCENTER");
                    //if (_state.LightRightConfig.GreaterThan && (_state.LightRight > _state.LightRightConfig.Threshold))
                        notify.Add("LIGHTRIGHT");

                    bool New_Get_All_Line_Left = response.Data[8] == 1;
                    bool New_Get_All_Line_Right = response.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.Data[10] == 1;
                    if (_state.Stall != newstall)
                    {
                        notify.Add("STALL");
                        _state.Stall = newstall;
                    }
                    break;
                case ScribblerHelper.Commands.GET_ALL_BINARY:
                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_LOUD:
                case ScribblerHelper.Commands.SET_QUIET:
                    ScribblerHelper.AllBinaryDecomp parse_all_binary = new ScribblerHelper.AllBinaryDecomp(response.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;
                    }
                    break;
                default:
                    LogError(LogGroups.Console, "Update State command missmatch");
                    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);
        }
Example #9
0
        /// <summary>
        /// Read Serial Port for a number of bytes and put into ScribblerResponse
        /// </summary>
        /// <param name="nBytes">number of bytes to read (includes the command type byte)</param>
        /// <returns>ScribblerResponse</returns>
        private ScribblerResponse GetCommandResponse(int nBytes)
        {
            byte[] inBuff = new byte[nBytes];
            ScribblerResponse response = null;
            int read = 0;
            try
            {
                while (read < nBytes)
                {
                    
                    int canread;
                    while ((canread = _serialPort.BytesToRead) == 0) ; //spin
                    int needtoread = nBytes - read;
                    if (canread > needtoread)
                    {
                        _serialPort.Read(inBuff, read, needtoread);
                        read += needtoread;
                    }
                    else
                    {
                        _serialPort.Read(inBuff, read, canread);
                        read += canread;
                    }
                    //DEBUG
                    //foreach (byte b in inBuff)
                    //{
                    //    if (b != 0)
                    //        Console.Write(b + " ");
                    //    else
                    //        Console.Write("` ");
                    //}
                    //Console.Write("\n");
                    //DEBUG
                }

                response = new ScribblerResponse(nBytes-1);
                response.CommandType = inBuff[inBuff.Length-1];
                for (int i = 0; i < inBuff.Length - 1; i++)
                    response.Data[i] = inBuff[i];

                //DEBUG
                //Console.Write("Got: ");
                //foreach (byte b in response.Data)
                //{
                //    if (b != 0)
                //        Console.Write(b + " ");
                //    else
                //        Console.Write("` ");
                //}
                //Console.Write("\n");
                //DEBUG
            }
            catch (Exception ex)
            {
                Console.WriteLine("GetCommandResponse Exception: " + ex);
            }
            return response;
        }
Example #10
0
        /// <summary>
        /// Read Serial Port for echo
        /// </summary>
        /// <param name="outBuff">The outbound message to match</param>
        /// <returns>ScribblerResponse</returns>
        private ScribblerResponse GetEcho(byte[] outBuff)
        {
            byte[] inBuff = new byte[outMessageSize];
            ScribblerResponse response = null;
            int ixOutBuff = 0;
            try
            {
                while (ixOutBuff < outMessageSize)
                {
                    byte[] temp = new byte[1];
                    _serialPort.Read(temp, 0, 1); //get 1 byte
                    if (temp[0] == outBuff[ixOutBuff])
                    {
                        inBuff[ixOutBuff] = temp[0];
                        ixOutBuff++;
                    }
                }

                response = new ScribblerResponse();
                response.Data = (byte[])inBuff.Clone();

                //DEBUG
                //Console.Write("Echo: ");
                //foreach (byte b in response.Data)
                //{
                //    if (b != 0)
                //        Console.Write(b + " ");
                //    else
                //        Console.Write("` ");
                //}
                //Console.Write("\n");
                //DEBUG
            }
            catch (Exception ex)
            {
                Console.WriteLine("GetCommandResponse Exception: " + ex);
            }
            return response;
        }