Beispiel #1
0
        public virtual IEnumerator <ITask> SendCommandHandler(SendCommand sendCommand)
        {
            LegoCommand cmd = sendCommand.Body.LegoCommand;

            if (buffer == null || _commState.SerialPort == null || !_commState.SerialPort.IsOpen)
            {
                sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Not Connected")));
                yield break;
            }
            if (sendCommand.Body == null || sendCommand.Body.LegoCommand == null)
            {
                sendCommand.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Invalid LEGO Command")));
                yield break;
            }

            // Dispatch again to one of three different ports:
            // If no response is required, send the command immediately
            // Otherwise queue into a low or high priority queue to
            // be handled one command/response at a time.
            if (!sendCommand.Body.LegoCommand.RequireResponse)
            {
                _commSendImmediatePort.Post(sendCommand);
            }
            else if (sendCommand.Body.PriorityRequest)
            {
                _legoPriorityRequestResponsePort.Post(sendCommand);
            }
            else
            {
                _legoRequestResponsePort.Post(sendCommand);
            }

            yield break;
        }
Beispiel #2
0
        /// <summary>
        /// Post Brick Send Command and return the response port.
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="priorityRequest"></param>
        /// <returns></returns>
        public virtual PortSet <LegoResponse, Fault> SendCommand(LegoCommand cmd, bool priorityRequest)
        {
            SendCommand op = new SendCommand();

            op.Body = new SendCommandRequest(cmd, priorityRequest);
            this.PostUnknownType(op);
            return(op.ResponsePort);
        }
Beispiel #3
0
        /// <summary>
        /// DO NOT USE THIS COMMAND DIRECTLY.
        /// In LegoNxt.cs, post a message to _legoCommWithAckPort or _legoCommNoAckPort.
        /// </summary>
        /// <param name="cmd"></param>
        /// <returns></returns>
        internal LegoResponse SendCommand(LegoCommand cmd)
        {
            LegoResponse response = null;

            if (buffer != null && serialPort != null && serialPort.IsOpen)
            {
                // Add the bluetooth packet length
                int ixBuffer = 0;
                // dataLength = commandType + commandCode + Data
                int packetLength = (2 + ((cmd.Data == null) ? 0 : cmd.Data.Length));
                buffer[ixBuffer++] = (byte)(packetLength % 256);
                buffer[ixBuffer++] = (byte)(packetLength / 256);

                // Add the bluetooth header bytes to get an actual packet count
                packetLength += 2;

                buffer[ixBuffer++] = (byte)(cmd.CommandType + ((!cmd.RequireResponse) ? 0x80 : 0x00));
                buffer[ixBuffer++] = (byte)cmd.LegoCommandCode;

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

                int dataLength = (cmd.Data == null) ? 0 : cmd.Data.Length;
                int ixData     = 0;
                while (ixData < dataLength || ixBuffer > 0)
                {
                    // Copy up to serialBufferLength from Data
                    while (ixBuffer < serialBufferLength && ixData < dataLength)
                    {
                        buffer[ixBuffer++] = cmd.Data[ixData++];
                    }

                    try
                    {
                        serialPort.Write(buffer, 0, ixBuffer);
                        ixBuffer = 0;
                    }
                    catch
                    {
                        _legoService.LogConsoleError("Serial Port Timeout.");
                        ShowLegoHelp(_legoService);
                        return(null);
                    }
                }

                response = GetCommandResponse(cmd);
            }
            return(response);
        }
Beispiel #4
0
 public SendLegoCommand(LegoCommand cmd)
 {
     this.Body = cmd;
 }
Beispiel #5
0
        /// <summary>
        /// Wait for a response on the serial port.
        /// </summary>
        /// <param name="getResponse"></param>
        /// <returns></returns>
        private IEnumerator <ITask> CommGetResponseHandler(GetResponse getResponse)
        {
            string             errorMessage = null;
            SendCommandRequest cmdRequest   = _commState.PendingRequests.Peek();

            if (cmdRequest == null)
            {
                yield break;
            }

            #region If the data isn't ready yet, wait a little and try again.
            if (_commState.SerialPort.BytesToRead == 0)
            {
                TimeSpan elapsed = (cmdRequest.Stopwatch == null) ? TimeSpan.MinValue : cmdRequest.Stopwatch.Elapsed;
                TimeSpan remaining;
                if (elapsed != TimeSpan.MinValue &&
                    elapsed < cmdRequest.MinExpectedTimeSpan)
                {
                    // Wait until the minimum expected time for this command.
                    remaining = cmdRequest.MinExpectedTimeSpan.Subtract(elapsed);
                }
                else if (elapsed.TotalSeconds < 1.0)
                {
                    // No data yet, wait 3 milliseconds
                    remaining = new TimeSpan(0, 0, 0, 0, 3);
                }
                else
                {
                    // Timeout has occurred
                    // Remove from the pending list
                    _commState.PendingRequests.Pop();

                    _commState.ConsecutiveReadTimeouts++;
                    if (_commState.ConsecutiveReadTimeouts > 5)
                    {
                        // Several read timeouts in a row means the serial port is no longer connected.
                        _state.Connected = false;
                        SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected));
                    }

                    errorMessage = "Timeout receiving data from the LEGO NXT.";

                    if (cmdRequest.InternalResponsePort != null)
                    {
                        Fault faultResponse = Fault.FromException(new IOException(errorMessage));
                        cmdRequest.InternalResponsePort.Post(faultResponse);
                    }
                    else
                    {
                        LogError(errorMessage);
                    }
                    yield break;
                }

                // Leave the exclusive handler,
                // but wake up in a little bit and try again.
                Activate(Arbiter.Receive(false, TimeoutPort(remaining),
                                         delegate(DateTime timeout)
                {
                    _commGetResponsePort.Post(GetResponse.Instance);
                }));
                yield break;
            }

            // When data starts to come in, clear the read timeout counter.
            if (_commState.ConsecutiveReadTimeouts > 0)
            {
                _commState.ConsecutiveReadTimeouts = 0;
            }

            #endregion

            // See if there is data on the serial port.
            // if not, post back to _commGetResponsePort
            // otherwise read it and send it back
            LegoCommand cmd       = cmdRequest.LegoCommand;
            bool        resetComm = false;
            try
            {
                int packetSize = cmd.ExpectedResponseSize;
                if (_state.ConnectOverBluetooth)
                {
                    packetSize = _commState.SerialPort.ReadByte() + (_commState.SerialPort.ReadByte() * 256);
                    if (packetSize != cmd.ExpectedResponseSize)
                    {
                        errorMessage = "Bluetooth header does not match the expected LEGO Command response size.";
                        resetComm    = true;
                    }
                }

                // Read the data and get a response packet.
                byte[] receiveData = new byte[packetSize];
                _commState.SerialPort.Read(receiveData, 0, packetSize);

                #region Timing Stats
                if (cmdRequest.Stopwatch != null)
                {
                    cmdRequest.Stopwatch.Stop();
                    _CodeTimerStats.Add(new CodeTimer(cmd.LegoCommandCode, cmdRequest.Stopwatch.Elapsed.TotalMilliseconds * 1000.0));
                    if (_CodeTimerStats.Count > 20)
                    {
                        ProcessCommunicationStats();
                    }
                }
                #endregion

                LegoResponse legoReceive = cmd.GetResponse(receiveData);

                byte commandType = (byte)(receiveData[0] & 0x7F);

                // Is this a valid starting type?
                if (commandType != 0x00 && commandType != 0x01 && commandType != 0x02)
                {
                    errorMessage = string.Format("Invalid LEGO response command: {0}", commandType);
                    resetComm    = true;
                }
                else
                {
                    // Data is received successfully
                    // Remove from the pending list
                    _commState.PendingRequests.Pop();

                    if (!_state.Connected)
                    {
                        _state.Connected = true;
                        SendNotification <ConnectionUpdate>(_subMgrPort, new ConnectionUpdate(_state.Connected));
                    }

                    if (cmdRequest.InternalResponsePort != null)
                    {
                        cmdRequest.InternalResponsePort.Post(legoReceive);
                    }

                    yield break;
                }
            }
            catch (ArgumentException ex)
            {
                resetComm = true;
                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.")
                {
                    errorMessage = "A connection error occured which may be caused by an invalid Baud Rate";
                }
                else
                {
                    errorMessage = "A connection error occured while accessing the LEGO NXT serial port";
                }
            }
            catch (TimeoutException)
            {
                // Ignore timeouts for now.

                errorMessage = "Timeout reading from LEGO NXT brick";
                resetComm    = true;
            }
            catch (IOException ex)
            {
                errorMessage = string.Format("Error reading from the serial port in NxtComm(): {0}", ex);
                resetComm    = true;
            }
            catch (Exception ex)
            {
                errorMessage = string.Format("Error reading from the serial port in NxtComm(): {0}", ex);
                resetComm    = true;
            }

            // Some error has occurred
            // Remove from the pending list
            _commState.PendingRequests.Pop();

            if (resetComm)
            {
                // A serious error occurred
                LogInfo(LogGroups.Console, "Resetting LEGO Serial Port");

                // Wait for remaining bytes
                yield return(Arbiter.Receive(false, TimeoutPort(300), delegate(DateTime timeout) { }));

                // Clear the serial port buffer
                _commState.SerialPort.DiscardInBuffer();
            }

            if (string.IsNullOrEmpty(errorMessage))
            {
                errorMessage = "Invalid or missing response data from LEGO NXT.";
            }

            if (cmdRequest.InternalResponsePort != null)
            {
                Fault faultResponse = Fault.FromException(new IOException(errorMessage));
                cmdRequest.InternalResponsePort.Post(faultResponse);
            }
            else
            {
                LogError(errorMessage);
            }

            yield break;
        }
Beispiel #6
0
 /// <summary>
 /// Lego Command Request
 /// </summary>
 /// <param name="legoCommand"></param>
 /// <param name="priorityRequest"></param>
 public SendCommandRequest(LegoCommand legoCommand, bool priorityRequest)
 {
     this.LegoCommand     = legoCommand;
     this.PriorityRequest = priorityRequest;
 }
Beispiel #7
0
 /// <summary>
 /// Lego Command Request
 /// </summary>
 /// <param name="legoCommand"></param>
 public SendCommandRequest(LegoCommand legoCommand)
 {
     this.LegoCommand = legoCommand;
 }
Beispiel #8
0
        /// <summary>
        /// Read Serial Port for a LEGO response
        /// </summary>
        /// <param name="cmd"></param>
        private LegoResponse GetCommandResponse(LegoCommand cmd)
        {
            if (cmd != null && cmd.RequireResponse && serialPort != null && serialPort.IsOpen)
            {
                int wait = 0;
                while (serialPort.BytesToRead == 0 && wait < 10)
                {
                    wait++;
                    if (wait < 10)
                    {
                        System.Threading.Thread.Sleep(100);
                    }
                    else
                    {
                        if (serialPort.CtsHolding && serialPort.DsrHolding)
                        {
                            // We might be connected in the wrong direction
                            _legoService.LogConsoleError("Have you connected from the LEGO NXT to the PC?");
                            ShowLegoHelp(_legoService);
                        }
                    }
                }

                try
                {
                    int  btLength    = serialPort.ReadByte() + (serialPort.ReadByte() * 256);
                    byte commandType = (byte)serialPort.ReadByte();
                    LegoHelper.LegoCommandCode command = (LegoHelper.LegoCommandCode)serialPort.ReadByte();
                    btLength -= 2;

                    if (cmd.LegoCommandCode != command)
                    {
                        // Garbled response.
                        // Wait for remaining bytes
                        // return an error
                        System.Threading.Thread.Sleep(500);
                        serialPort.DiscardInBuffer();
                        IOException ex1 = new IOException("Garbled data received from LEGO NXT.  LEGO response code does not match LEGO command.");
                        return(new LegoResponseException(cmd, ex1));
                    }

                    LegoResponse legoReceive = new LegoResponse(commandType, command, btLength);

                    if (btLength > 0)
                    {
                        legoReceive.Data = new byte[btLength];
                        serialPort.Read(legoReceive.Data, 0, btLength);
                    }

                    // Is this a valid starting type?
                    if (commandType != 0x00 && commandType != 0x01 &&
                        commandType != 0x02 && commandType != 0x80 &&
                        commandType != 0x81)
                    {
                        _legoService.LogConsoleInfo(string.Format("Invalid LEGO response command: {0}", commandType));
                    }

                    return(legoReceive);
                }
                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.")
                    {
                        _legoService.LogConsoleError("A connection error occured which may be caused by an invalid Baud Rate");
                        IOException ex3 = new IOException("Invalid Baud Rate");
                        return(new LegoResponseException(cmd, ex3));
                    }
                    else
                    {
                        _legoService.LogConsoleError("A connection error occured while accessing the LEGO NXT serial port: " + ex.Message);
                        return(new LegoResponseException(cmd, ex));
                    }
                }
                catch (TimeoutException ex)
                {
                    // Ignore timeouts for now.
                    _legoService.LogConsoleInfo("Timeout reading from LEGO NXT brick");
                    return(new LegoResponseException(cmd, ex));
                }
                catch (IOException ex)
                {
                    _legoService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
                    return(new LegoResponseException(cmd, ex));
                }
                catch (Exception ex)
                {
                    _legoService.LogConsoleError(string.Format("Error reading from the serial port in CommBase(): {0}", ex.Message));
                    return(new LegoResponseException(cmd, ex));
                }
            }

            LegoResponse response = new LegoResponse(2, cmd.LegoCommandCode, 1);

            if (cmd == null || cmd.RequireResponse)
            {
                response.ErrorCode = LegoErrorCode.UnknownStatus;

                if (cmd == null)
                {
                    _legoService.LogConsoleError("Invalid LEGO Command: null");
                }
                else if (serialPort == null || !serialPort.IsOpen)
                {
                    _legoService.LogConsoleWarning(string.Format("Unable to retrieve response from LEGO command {0} because the serial port is not open.", cmd.LegoCommandCode));
                }
            }

            return(response);
        }
        /// <summary>
        /// Send Any Command, including a two-phase LowSpeed command (LSWrite)
        /// </summary>
        /// <param name="cmd"></param>
        /// <param name="priority"></param>
        /// <returns></returns>
        public PortSet<LegoResponse, Fault> SendAnyCommand(LegoCommand cmd, bool priority)
        {
            if (cmd.LegoCommandCode == LegoCommandCode.LSWrite)
            {
                LegoLSWrite cmdLsWrite = cmd as LegoLSWrite;
                if (cmdLsWrite == null)
                    cmdLsWrite = new LegoLSWrite(cmd);

                // Send Low Speed Command
                SendLowSpeedCommand sendCmd = new SendLowSpeedCommand(cmdLsWrite);
                _internalI2CPort.Post(sendCmd);
                return sendCmd.ResponsePort;
            }
            return _brickPort.SendCommand(cmd, priority);
        }