Ejemplo n.º 1
0
        /// <summary>
        /// Send a command to the Scribbler and wait for a response.
        /// </summary>
        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
            {
                //reset timer
                PollTimer.Enabled = false;
                PollTimer.Enabled = true;

                //Update our state with the scribbler's response
                //UpdateState(validResponse);
                _mainPort.Post(new ScribblerResponseMessage(validResponse));

                command.ResponsePort.Post(validResponse);
            }

            // Ready to process another command
            Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));
            yield break;
        }
Ejemplo n.º 2
0
        /// <summary>
        /// Handles incoming SetAllLEDs requests
        /// </summary>
        private IEnumerator <ITask> SetAllLEDsHandler(SetAllLEDs message)
        {
            //error check
            if (!_state.Connected)
            {
                message.ResponsePort.Post(new Fault());
                yield break;
            }

            //update state
            _state.LEDLeft   = message.Body.LeftLED;
            _state.LEDCenter = message.Body.CenterLED;
            _state.LEDRight  = message.Body.RightLED;

            //send command
            ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL,
                                                        _state.LEDLeft,
                                                        _state.LEDCenter,
                                                        _state.LEDRight);

            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
            }
                                                             ));

            //reply to sender
            message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
Ejemplo n.º 3
0
        /// <summary>
        /// Handles incoming play tone requests
        /// </summary>
        private IEnumerator <ITask> PlayToneHandler(PlayTone message)
        {
            if (!_state.Connected)
            {
                LogError("trying to play tone, but not connected");
                message.ResponsePort.Post(new Fault());
                yield break;
            }

            ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_SPEAKER_2,
                                                        message.Body.Duration,
                                                        message.Body.Frequency1,
                                                        message.Body.Frequency2);

            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
                //reply to sender
                message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            }
                                                             ));


            yield break;
        }
Ejemplo n.º 4
0
        /// <summary>
        /// This will poll the scribbler at a minimum frequency
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void PollTimer_Elapsed(object sender, EventArgs e)
        {
            ScribblerCommand     cmd     = new ScribblerCommand((byte)ScribblerHelper.Commands.GET_ALL);
            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);
        }
Ejemplo n.º 5
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;
        }
Ejemplo n.º 6
0
        /// <summary>
        /// Handles incoming set motor messages
        /// </summary>
        private IEnumerator <ITask> SetMotorHandler(SetMotors message)
        {
            if (!_state.Connected)
            {
                message.ResponsePort.Post(new Fault());
                yield break;
            }

            //debug
            if (message.Body.LeftSpeed < 0 || message.Body.LeftSpeed > 200 || message.Body.RightSpeed < 0 || message.Body.RightSpeed > 200)
            {
                LogError("Scribbler SetMotorHandler: target power set incorrect");
            }

            //update state
            _state.MotorLeft  = message.Body.LeftSpeed;
            _state.MotorRight = message.Body.RightSpeed;

            //send command
            ScribblerCommand     cmd     = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
                //initialize notify list
                List <string> notify = new List <string>();
                notify.Add("MOTORS");

                // 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);

                //reply to say we are done
                message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            }
                                                             ));

            yield break;
        }
Ejemplo n.º 7
0
        private IEnumerator <ITask> SetNameHandler(SetName command)
        {
            if (string.IsNullOrEmpty(command.Body.NewName))
            {
                LogError("New name is null");
                command.ResponsePort.Post(new Fault());
                yield break;
            }

            if (!_state.Connected)
            {
                command.ResponsePort.Post(new Fault());
                yield break;
            }

            string shortenedname;

            if (command.Body.NewName.Length > 8)
            {
                shortenedname = command.Body.NewName.Substring(0, 8);
            }
            else
            {
                shortenedname = command.Body.NewName;
            }

            _state.RobotName = shortenedname;

            ScribblerCommand     cmd     = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_NAME, shortenedname);
            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
                SaveState(_state);
            }
                                                             ));

            //reply to sender
            command.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
Ejemplo n.º 8
0
        /// <summary>
        /// Handles incoming play tone requests
        /// </summary>
        private IEnumerator <ITask> PlayToneHandler(PlayTone message)
        {
            ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_SPEAKER_2,
                                                        message.Body.Duration,
                                                        message.Body.Frequency1,
                                                        message.Body.Frequency2);

            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
            }
                                                             ));

            //reply to sender
            message.ResponsePort.Post(DefaultUpdateResponseType.Instance);

            yield break;
        }
Ejemplo n.º 9
0
        public IEnumerator <ITask> HttpPostHandler(HttpPost httpPost)
        {
            // Use helper to read form data
            ReadFormData readForm = new ReadFormData(httpPost.Body.Context);

            _httpUtilities.Post(readForm);

            // Wait for result
            Activate(Arbiter.Choice(readForm.ResultPort,
                                    delegate(NameValueCollection parameters)
            {
                if (!string.IsNullOrEmpty(parameters["Action"]) &&
                    parameters["Action"] == "ScribblerConfig")
                {
                    if (parameters["buttonOk"] == "Change" && _state.Connected)
                    {
                        SetNameBody newname    = new SetNameBody(parameters["Name"]);
                        SetName newnamemessage = new SetName(newname);
                        _mainPort.Post(newnamemessage);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <DefaultUpdateResponseType>(false, newnamemessage.ResponsePort,
                                                                            delegate(DefaultUpdateResponseType response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, newnamemessage.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                    else if (parameters["buttonOk"] == "Connect" && _state.Connected)
                    {
                        //close down this connection to make a new connection below

                        PollTimer.Close();

                        System.Threading.Thread.Sleep(100);

                        _scribblerCom.Close();

                        _state.Connected = false;
                    }

                    if (parameters["buttonOk"] == "Connect" && !_state.Connected)
                    {
                        int port = 0;
                        int.TryParse(parameters["ComPort"], out port);
                        string name = parameters["Name"];
                        if (!string.IsNullOrEmpty(name) && name.Length > 8)
                        {
                            name = name.Substring(0, 8);
                        }

                        _state.ComPort   = port;
                        _state.RobotName = name;

                        //open Scribbler Communications port
                        if (ConnectToScribbler())
                        {
                            // Listen for a single Serial port request with an acknowledgement
                            Activate(Arbiter.ReceiveWithIterator <SendScribblerCommand>(false, _scribblerComPort, SendScribblerCommandHandler));

                            PollTimer           = new System.Timers.Timer();
                            PollTimer.Interval  = TimerDelay;
                            PollTimer.AutoReset = true;
                            PollTimer.Elapsed  += new System.Timers.ElapsedEventHandler(PollTimer_Elapsed);
                            PollTimer.Start();

                            //play startup tone
                            PlayToneBody startTone   = new PlayToneBody(200, 1000, 2000);
                            PlayTone playToneMessage = new PlayTone(startTone);
                            _mainPort.Post(playToneMessage);

                            Activate(
                                Arbiter.Choice(
                                    Arbiter.Receive <DefaultUpdateResponseType>(false, playToneMessage.ResponsePort,
                                                                                delegate(DefaultUpdateResponseType response)
                            {
                                HttpPostSuccess(httpPost);
                            }),
                                    Arbiter.Receive <Fault>(false, playToneMessage.ResponsePort,
                                                            delegate(Fault f)
                            {
                                HttpPostFailure(httpPost, f.Reason[0].Value);
                            })
                                    )
                                );
                        }
                        else
                        {
                            HttpPostFailure(httpPost, "Connection to Scribbler failed");
                        }
                    }
                }
                else if (!string.IsNullOrEmpty(parameters["Action"]) &&
                         parameters["Action"] == "ScribblerSensors")
                {
                    if (parameters["buttonOk"] == "Poll" && _state.Connected)
                    {
                        ScribblerCommand cmd         = new ScribblerCommand((byte)ScribblerHelper.Commands.GET_ALL);
                        SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);
                        _scribblerComPort.Post(sendcmd);
                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                                    delegate(ScribblerResponse response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, sendcmd.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                }
                else if (!string.IsNullOrEmpty(parameters["Action"]) &&
                         parameters["Action"] == "ScribblerMotors")
                {
                    if (parameters["buttonOk"] == "Set" && _state.Connected)
                    {
                        int left  = _state.MotorLeft;
                        int right = _state.MotorRight;
                        int.TryParse(parameters["LeftMotor"], out left);
                        int.TryParse(parameters["RightMotor"], out right);

                        SetMotorsBody setMotorsBody = new SetMotorsBody(left, right);
                        SetMotors setMotorsRequest  = new SetMotors(setMotorsBody);

                        _mainPort.Post(setMotorsRequest);

                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <DefaultUpdateResponseType>(false, setMotorsRequest.ResponsePort,
                                                                            delegate(DefaultUpdateResponseType response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, setMotorsRequest.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                    else if (parameters["buttonOk"] == "All Stop" && _state.Connected)
                    {
                        SetMotorsBody setMotorsBody = new SetMotorsBody(100, 100);
                        SetMotors setMotorsRequest  = new SetMotors(setMotorsBody);

                        _mainPort.Post(setMotorsRequest);

                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <DefaultUpdateResponseType>(false, setMotorsRequest.ResponsePort,
                                                                            delegate(DefaultUpdateResponseType response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, setMotorsRequest.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                }
                else if (!string.IsNullOrEmpty(parameters["Action"]) &&
                         parameters["Action"] == "ScribblerLEDs")
                {
                    if (parameters["buttonOk"] == "Set" && _state.Connected)
                    {
                        bool left   = ((parameters["LeftLED"] ?? "off") == "on");
                        bool center = ((parameters["CenterLED"] ?? "off") == "on");
                        bool right  = ((parameters["RightLED"] ?? "off") == "on");

                        SetAllLedsBody leds   = new SetAllLedsBody(left, center, right);
                        SetAllLEDs setAllLeds = new SetAllLEDs(leds);
                        _mainPort.Post(setAllLeds);

                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <DefaultUpdateResponseType>(false, setAllLeds.ResponsePort,
                                                                            delegate(DefaultUpdateResponseType response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, setAllLeds.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                }
                else if (!string.IsNullOrEmpty(parameters["Action"]) &&
                         parameters["Action"] == "ScribblerSpeaker")
                {
                    if (parameters["buttonOk"] == "Play" && _state.Connected)
                    {
                        int tone1    = 0;
                        int tone2    = 0;
                        int duration = 0;
                        int.TryParse(parameters["Tone1"], out tone1);
                        int.TryParse(parameters["Tone2"], out tone2);
                        int.TryParse(parameters["Duration"], out duration);

                        PlayToneBody playTone    = new PlayToneBody(duration, tone1, tone2);
                        PlayTone playToneMessage = new PlayTone(playTone);
                        _mainPort.Post(playToneMessage);

                        Activate(
                            Arbiter.Choice(
                                Arbiter.Receive <DefaultUpdateResponseType>(false, playToneMessage.ResponsePort,
                                                                            delegate(DefaultUpdateResponseType response)
                        {
                            HttpPostSuccess(httpPost);
                        }),
                                Arbiter.Receive <Fault>(false, playToneMessage.ResponsePort,
                                                        delegate(Fault f)
                        {
                            HttpPostFailure(httpPost, f.Reason[0].Value);
                        })
                                )
                            );
                    }
                }
                else
                {
                    HttpPostFailure(httpPost, "Unknown Http Post");
                }
            },
                                    delegate(Exception Failure)
            {
                LogError(Failure.Message);
            })
                     );
            yield break;
        }
Ejemplo n.º 10
0
        /// <summary>
        /// Handles incoming SetLED requests
        /// </summary>
        private IEnumerator <ITask> SetLEDHandler(SetLED message)
        {
            if (!_state.Connected)
            {
                LogError("Trying to set LED, but not connected");
                message.ResponsePort.Post(new Fault());
                yield break;
            }

            ScribblerCommand cmd;

            switch (message.Body.LED)
            {
            case 0:     //left LED
                _state.LEDLeft = message.Body.State;
                if (message.Body.State)
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_LEFT_ON);
                }
                else
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_LEFT_OFF);
                }
                break;

            case 1:     //center LED
                _state.LEDCenter = message.Body.State;
                if (message.Body.State)
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_CENTER_ON);
                }
                else
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_CENTER_OFF);
                }
                break;

            case 2:     //right LED
                _state.LEDRight = message.Body.State;
                if (message.Body.State)
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_RIGHT_ON);
                }
                else
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_RIGHT_OFF);
                }
                break;

            case 3:     //all LEDs
                _state.LEDLeft   = message.Body.State;
                _state.LEDCenter = message.Body.State;
                _state.LEDRight  = message.Body.State;
                if (message.Body.State)
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL_ON);
                }
                else
                {
                    cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_LED_ALL_OFF);
                }
                break;

            default:
                LogError("LED number set incorrect");
                cmd = new ScribblerCommand();
                break;
            }

            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
            }
                                                             ));

            //reply to sender
            message.ResponsePort.Post(DefaultUpdateResponseType.Instance);
            yield break;
        }
Ejemplo n.º 11
0
        /// <summary>
        /// Handles incoming set motor messages
        /// </summary>
        private IEnumerator <ITask> SetMotorHandler(SetMotor message)
        {
            if (message.Body == null)
            {
                message.ResponsePort.Post(new Fault());
                yield break;
            }
            if (message.Body.Motor == null)
            {
                message.ResponsePort.Post(new Fault());
                yield break;
            }

            // Requests come too fast, so dump ones that come in too fast.
            if (RequestPending > 0 && message.Body.Speed != 100)
            {
                message.ResponsePort.Post(new DefaultUpdateResponseType());
                yield break;
            }

            RequestPending++;

            if (message.Body.Motor.ToUpper().Contains("LEFT"))
            {
                _state.MotorLeft = message.Body.Speed;
            }
            else if (message.Body.Motor.ToUpper().Contains("RIGHT"))
            {
                _state.MotorRight = message.Body.Speed;
            }
            else
            {
                LogError("Motor name not set properly");
            }


            ScribblerCommand     cmd     = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft);
            SendScribblerCommand sendcmd = new SendScribblerCommand(cmd);

            _scribblerComPort.Post(sendcmd);

            yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort,
                                                             delegate(ScribblerResponse response)
            {
                //if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS)
                //    LogError("SetMotor picked up a wrong echo");

                //initialize notify list
                List <string> notify = new List <string>();
                notify.Add("MOTORS");

                // 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);

                //reply to say we are done
                message.ResponsePort.Post(DefaultUpdateResponseType.Instance);

                RequestPending--;
            }
                                                             ));

            yield break;
        }