コード例 #1
0
        /// <summary>
        /// Handles incoming sensor messages
        /// </summary>
        private IEnumerator <ITask> SensorNotificationHandler(SensorNotification message)
        {
            //initialize notify list
            List <string> notify = new List <string>();

            //update state
            switch (ScribblerHelper.SensorType((byte)message.Sensor))
            {
            case "IRLeft":
                _state.IRLeft = (message.Status > 0);
                notify.Add("IRLEFT");
                break;

            case "IRRight":
                _state.IRRight = (message.Status > 0);
                notify.Add("IRRIGHT");
                break;

            case "Stall":
                _state.Stall = (message.Status > 0);
                notify.Add("STALL");
                break;

            case "LineLeft":
                _state.LineLeft = (message.Status > 0);
                notify.Add("LINELEFT");
                break;

            case "LineRight":
                _state.LineRight = (message.Status > 0);
                notify.Add("LINERIGHT");
                break;

            case "LightLeft":
                _state.LightLeft = message.Status;
                notify.Add("LIGHTLEFT");
                break;

            case "LightRight":
                _state.LightRight = message.Status;
                notify.Add("LIGHTRIGHT");
                break;

            case "LightCenter":
                _state.LightCenter = message.Status;
                notify.Add("LIGHTCENTER");
                break;

            //only notify if there is a change
            case "AllBinary":
                ScribblerHelper.AllBinaryDecomp newState = new ScribblerHelper.AllBinaryDecomp(message.Status);
                if (newState.IRLeft != _state.IRLeft)
                {
                    notify.Add("IRLEFT");
                    _state.IRLeft = newState.IRLeft;
                }
                if (newState.IRRight != _state.IRRight)
                {
                    notify.Add("IRRIGHT");
                    _state.IRRight = newState.IRRight;
                }
                if (newState.Stall != _state.Stall)
                {
                    notify.Add("STALL");
                    _state.Stall = newState.Stall;
                }
                if (newState.LineLeft != _state.LineLeft)
                {
                    notify.Add("LINELEFT");
                    _state.LineLeft = newState.LineLeft;
                }
                if (newState.LineRight != _state.LineRight)
                {
                    notify.Add("LINERIGHT");
                    _state.LineRight = newState.LineRight;
                }
                break;

            default:
                LogError("Unrecognized sensor type");
                //throw new ArgumentException("Sensor update error");
                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;
        }
コード例 #2
0
        /// <summary>
        /// Serial Port data event handler
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        void serialPort_DataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            if (e.EventType == SerialData.Chars)
            {
                try
                {
                    while (serialPort.BytesToRead >= 2)
                    {
                        int messageLength = serialPort.ReadByte();

                        while ((messageLength > 5 || messageLength == 0) && serialPort.BytesToRead > 0)
                        {
                            messageLength = serialPort.ReadByte();
                        }

                        if (messageLength >= 2)
                        {
                            byte[] data = new byte[messageLength];

                            serialPort.Read(data, 0, messageLength);

                            //debug
                            foreach (byte b in data)
                            {
                                if (b != 0)
                                {
                                    Console.Write(b + " ");
                                }
                                else
                                {
                                    Console.Write("` ");
                                }
                            }
                            Console.Write("\n");

                            if (ScribblerHelper.IsSensorResponse(data[0]))
                            {
                                SensorNotification sensorMsg = new SensorNotification(data[0], data[1]);

                                ScribblerComInboundPort.Post(sensorMsg);
                            }
                            else
                            {
                                ScribblerCommand echo = new ScribblerCommand();
                                echo.Data = data;
                                ScribblerComInboundPort.Post(echo);
                            }
                        }
                    }
                }
                catch (ArgumentException ex)
                {
                    if (ex.Message == "Offset and length were out of bounds for the array or count is greater than the number of elements from index to the end of the source collection.")
                    {
                        Exception invalidBaudRate = new IOException();
                        System.Diagnostics.Debug.WriteLine("Invalid Baud Rate");
                        ScribblerComInboundPort.Post(invalidBaudRate);
                    }
                    else
                    {
                        Console.WriteLine("Error reading from the serial port in ComBase(): {0}", ex.Message);
                        System.Diagnostics.Debug.WriteLine("Error reading from the serial port in ComBase(): {0}", ex.Message);
                        ScribblerComInboundPort.Post(ex);
                    }
                }
                catch (TimeoutException ex)
                {
                    ScribblerComInboundPort.Post(ex);
                    // Ignore timeouts for now.
                }
                catch (IOException ex)
                {
                    Console.WriteLine("Error reading from the serial port in CommBase(): {0}", ex.Message);
                    ScribblerComInboundPort.Post(ex);
                }
                catch (Exception ex)
                {
                    Console.WriteLine("Error reading from the serial port in CommBase(): {0}", ex.Message);
                    ScribblerComInboundPort.Post(ex);
                }
            }
        }