Пример #1
0
        public IEnumerator <ITask> DriveAllStopHandler(drive.AllStop allstop)
        {
            this.state.DriveState.TimeStamp = DateTime.Now;

            serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
            sg.Timeout = this.state.DefaultResponsePause;
            sg.Data    = new serialcomservice.Packet(board.CreatePacket <sbyte>(board.SetFullStopString));

            var resultPort = this.serialCOMServicePort.SendAndGet(sg);

            yield return(resultPort.Choice());

            soap.Fault f = (soap.Fault)resultPort;
            if (f != null)
            {
                LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                allstop.ResponsePort.Post(f);
                yield break;
            }

            if (this.HasFWError((serialcomservice.Packet)resultPort))
            {
                f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
                allstop.ResponsePort.Post(f);
                yield break;
            }

            // Reflect into our state
            this.state.DriveState.LeftWheel.MotorState.CurrentPower  = GDDriveFullStop;
            this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop;
            this.state.DriveState.LeftWheel.WheelSpeed  = 0;
            this.state.DriveState.RightWheel.WheelSpeed = 0;
            this.state.DriveState.IsEnabled             = false;
            allstop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #2
0
        /// <summary>
        /// Keep the FW watchdog alive by continuously retrieving sensor+encoder values
        /// </summary>
        /// <param name="dt">A instance of type DateTime</param>
        /// <returns>A instance of IEnumerator of type ITask</returns>
        private IEnumerator <ITask> PollSensors(DateTime dt)
        {
            try
            {
                soap.Fault           fault         = null;
                adc.ADCPinArrayState pinArrayState = null;

                adc.Get adcGet = new adc.Get();

                this.controllerAdcPinArrayPort.Post(adcGet);

                yield return(adcGet.ResponsePort.Choice());

                fault = (soap.Fault)adcGet.ResponsePort;

                if (fault != null)
                {
                    LogError(string.Format("Fault on getting ADCPinArray state: {0}", fault.Detail));
                    throw fault.ToException();
                }

                pinArrayState = (adc.ADCPinArrayState)adcGet.ResponsePort;

                for (int i = 0; i < this.state.InfraredSensorState.Sensors.Count; i++)
                {
                    adc.ADCPinState pinState = pinArrayState.Pins[this.state.InfraredSensorState.Sensors[i].HardwareIdentifier];

                    this.state.InfraredSensorState.Sensors[i].DistanceMeasurement = this.state.InfraredRawValueDivisorScalar * Math.Pow(pinState.PinValue, this.state.InfraredDistanceExponent) / CentimetersPerMeter;
                }

                for (int i = 0; i < this.state.SonarSensorState.Sensors.Count; i++)
                {
                    adc.ADCPinState pinState = pinArrayState.Pins[this.state.SonarSensorState.Sensors[i].HardwareIdentifier];

                    this.state.SonarSensorState.Sensors[i].DistanceMeasurement = pinState.PinValue * this.state.SonarTimeValueMultiplier / CentimetersPerMeter;
                }

                this.state.BatteryState.PercentBatteryPower = (pinArrayState.Pins[this.state.BatteryVoltagePinIndex].PinValue * this.state.BatteryVoltageDivider) / this.state.BatteryState.MaxBatteryPower * 100.0;

                if (this.state.BatteryState.PercentBatteryPower <= this.state.BatteryState.PercentCriticalBattery)
                {
                    battery.BatteryNotification batteryNotification = new battery.BatteryNotification(
                        (int)this.state.BatteryState.MaxBatteryPower,
                        this.state.BatteryState.PercentBatteryPower);

                    this.SendNotification <battery.Replace>(this.submgrBatteryPort, batteryNotification);
                }
            }
            finally
            {
                // Ensure we haven't been droppped
                if (ServicePhase == ServiceRuntimePhase.Started)
                {
                    // Issue another polling request
                    Activate(TimeoutPort(this.state.SensorPollingInterval).Receive(this.sensorPollingPort.Post));
                }
            }

            yield break;
        }
Пример #3
0
            void OnSetSrgsGrammarFileFault(soap.Fault response)
            {
                texttospeech.SayTextRequest request = new texttospeech.SayTextRequest();
                request.SpeechText = @"Could not load the grammar file. The error is as follows: " + response.Reason[response.Reason.Length - 1].Value;
                TexttoSpeechTTSPort.SayText(request);

                Decrement();
            }
Пример #4
0
        private IEnumerator <ccr.ITask> DoStart()
        {
            soap.Fault fault = null;

            // Subscribe to partners

            yield return(ccr.Arbiter.Choice(
                             _speechRecognizerPort.Subscribe(_speechRecognizerNotify,
                                                             typeof(Microsoft.Robotics.Technologies.Speech.SpeechRecognizer.Proxy.SpeechRecognized),
                                                             typeof(Microsoft.Robotics.Technologies.Speech.SpeechRecognizer.Proxy.SpeechRecognitionRejected)
                                                             ),
                             EmptyHandler,
                             delegate(soap.Fault f)
            {
                fault = f;
            }
                             ));

            if (fault != null)
            {
                LogError("Failed to subscribe to program.SpeechRecognizer.SpeechRecognizer", fault);
                StartFailed();
                yield break;
            }

            // Start the RunHandler, this represents the parts of the diagram that
            // are are not run in the context of an operation or notification.
            StartHandler start = new StartHandler(this, Environment.TaskQueue);

            SpawnIterator(start.RunHandler);
            // Wait until the RunHandler has completed.
            yield return(ccr.Arbiter.Receive(false, start.Complete, EmptyHandler));

            // Start operation handlers and insert into directory service.
            StartHandlers();

            // Add notifications to the main interleave
            base.MainPortInterleave.CombineWith(
                new ccr.Interleave(
                    new ccr.ExclusiveReceiverGroup(
                        ),
                    new ccr.ConcurrentReceiverGroup(
                        )
                    )
                );

            // Activate independent tasks
            Activate <ccr.ITask>(
                ccr.Arbiter.ReceiveWithIterator <Microsoft.Robotics.Technologies.Speech.SpeechRecognizer.Proxy.SpeechRecognized>(true, _speechRecognizerNotify, SpeechRecognizerSpeechRecognizedHandler),
                ccr.Arbiter.ReceiveWithIterator <Microsoft.Robotics.Technologies.Speech.SpeechRecognizer.Proxy.SpeechRecognitionRejected>(true, _speechRecognizerNotify, SpeechRecognizerSpeechRecognitionRejectedHandler)
                );

            yield break;
        }
Пример #5
0
        public IEnumerator <ITask> DriveDriveDistanceHandler(drive.DriveDistance drivedistance)
        {
            if (!this.state.DriveState.IsEnabled)
            {
                drivedistance.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
                                                    soap.FaultCodes.Receiver,
                                                    DsspFaultCodes.OperationFailed,
                                                    "Drive not enabled!"));
                yield break;
            }

            this.state.DriveState.TimeStamp = DateTime.Now;

            short pwr = (short)(board.HBridgeReverseMax +
                                ((drivedistance.Body.Power - GDDriveMin)
                                 * DrivePowerScale));

            short dist = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * drivedistance.Body.Distance);

            byte[] cmdpacket = board.CreatePacket <short, short>(board.SetTravelDistanceString, dist, pwr);

            serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
            sg.Timeout = this.state.DefaultResponsePause;
            sg.Data    = new serialcomservice.Packet(cmdpacket);

            var resultPort = this.serialCOMServicePort.SendAndGet(sg);

            yield return(resultPort.Choice());

            soap.Fault f = (soap.Fault)resultPort;
            if (f != null)
            {
                LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                drivedistance.ResponsePort.Post(f);
                yield break;
            }

            if (this.HasFWError((serialcomservice.Packet)resultPort))
            {
                f = soap.Fault.FromCodeSubcodeReason(
                    soap.FaultCodes.Receiver,
                    DsspFaultCodes.OperationFailed,
                    "Error received from FW!");
                drivedistance.ResponsePort.Post(f);
                yield break;
            }

            this.state.DriveState.LeftWheel.MotorState.CurrentPower  = drivedistance.Body.Power;
            this.state.DriveState.RightWheel.MotorState.CurrentPower = drivedistance.Body.Power;
            this.state.DriveState.DriveDistanceStage = drive.DriveStage.Completed;
            drivedistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #6
0
        /// <summary>
        /// Performs a reset on both encoders in response to
        /// a reset (update) on either encoder.
        /// </summary>
        /// <param name="reset">An encoder Reset operation type</param>
        /// <returns>An IEnumerator of type ITask</returns>
        private IEnumerator <ITask> InternalEncoderReset(drive.ResetEncoders reset)
        {
            serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
            sg.Timeout    = this.state.DefaultResponsePause;
            sg.Terminator = board.PacketTerminatorString;
            sg.Data       = new serialcomservice.Packet(board.CreatePacket <byte>(board.ResetEncoderTicksString));

            PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg);

            yield return(resultPort.Choice());

            soap.Fault f = (soap.Fault)resultPort;
            if (f != null)
            {
                reset.ResponsePort.Post(f);
                yield break;
            }

            serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
            if (this.HasFWError(p))
            {
                f = soap.Fault.FromCodeSubcodeReason(
                    soap.FaultCodes.Receiver,
                    DsspFaultCodes.OperationFailed,
                    "FW ERROR: Failed to reset encoder tick count!");
                reset.ResponsePort.Post(f);
                yield break;
            }

            // update both the encoder partner state as well as the brick state
            this.state.DriveState.LeftWheel.EncoderState.TicksSinceReset      =
                this.state.DriveState.RightWheel.EncoderState.TicksSinceReset = 0;

            this.state.DriveState.LeftWheel.EncoderState.CurrentAngle      =
                this.state.DriveState.RightWheel.EncoderState.CurrentAngle = 0;

            this.state.DriveState.LeftWheel.EncoderState.CurrentReading      =
                this.state.DriveState.RightWheel.EncoderState.CurrentReading = 0;

            this.state.DriveState.LeftWheel.EncoderState.TimeStamp      =
                this.state.DriveState.RightWheel.EncoderState.TimeStamp = DateTime.Now;

            // Notify any brick subscribers as well as any drive subscribers
            this.SendNotification <Replace>(this.submgrPort, new Replace());
            this.SendNotification <drive.ResetEncoders>(this.submgrDrivePort, new drive.ResetEncoders());

            reset.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #7
0
        public IEnumerator <ITask> DriveSetDriveSpeedHandler(drive.SetDriveSpeed setdrivespeed)
        {
            if (!this.state.DriveState.IsEnabled)
            {
                setdrivespeed.ResponsePort.Post(soap.Fault.FromCodeSubcodeReason(
                                                    soap.FaultCodes.Receiver,
                                                    DsspFaultCodes.OperationFailed,
                                                    "Drive not enabled!"));
                yield break;
            }

            this.state.DriveState.TimeStamp = DateTime.Now;

            short leftWheelSpeed  = (short)(this.encoderTicksPerMeter[(int)Sides.Left] * setdrivespeed.Body.LeftWheelSpeed);
            short rightWheelSpeed = (short)(this.encoderTicksPerMeter[(int)Sides.Right] * setdrivespeed.Body.RightWheelSpeed);

            byte[] cmdpacket = board.CreatePacket <short>(board.SetTravelVelocityString, leftWheelSpeed, rightWheelSpeed);

            serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
            sg.Timeout = this.state.DefaultResponsePause;
            sg.Data    = new serialcomservice.Packet(cmdpacket);

            var resultPort = this.serialCOMServicePort.SendAndGet(sg);

            yield return(resultPort.Choice());

            soap.Fault f = (soap.Fault)resultPort;
            if (f != null)
            {
                LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                setdrivespeed.ResponsePort.Post(f);
                yield break;
            }

            if (this.HasFWError((serialcomservice.Packet)resultPort))
            {
                f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
                setdrivespeed.ResponsePort.Post(f);
                yield break;
            }

            this.state.DriveState.LeftWheel.WheelSpeed  = setdrivespeed.Body.LeftWheelSpeed;
            this.state.DriveState.RightWheel.WheelSpeed = setdrivespeed.Body.RightWheelSpeed;
            setdrivespeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #8
0
        public IEnumerator <ITask> DriveEnableDriveHandler(drive.EnableDrive enabledrive)
        {
            this.state.DriveState.TimeStamp = DateTime.Now;
            this.state.DriveState.IsEnabled = enabledrive.Body.Enable;

            // The caller's intent is to disable the drive, so we need to set motor power to zero
            if (!this.state.DriveState.IsEnabled)
            {
                serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
                sg.Timeout = this.state.DefaultResponsePause;
                sg.Data    = new serialcomservice.Packet(board.CreatePacket <byte>(board.SetFullStopString));

                var resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                soap.Fault f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", board.SetFullStopString));
                    enabledrive.ResponsePort.Post(f);
                    yield break;
                }

                if (this.HasFWError((serialcomservice.Packet)resultPort))
                {
                    f = soap.Fault.FromCodeSubcodeReason(soap.FaultCodes.Receiver, DsspFaultCodes.OperationFailed, "Error received from FW!");
                    enabledrive.ResponsePort.Post(f);
                    yield break;
                }

                // Reflect into state
                this.state.DriveState.LeftWheel.MotorState.CurrentPower  = GDDriveFullStop;
                this.state.DriveState.RightWheel.MotorState.CurrentPower = GDDriveFullStop;
                this.state.DriveState.LeftWheel.WheelSpeed  = 0;
                this.state.DriveState.RightWheel.WheelSpeed = 0;
            }

            enabledrive.ResponsePort.Post(DefaultUpdateResponseType.Instance);
        }
Пример #9
0
        public IEnumerator <ITask> HttpGetHandler(HttpGet httpGet)
        {
            Get get = new Get();

            soap.Fault fault = null;

            this.mainPort.Post(get);

            yield return(get.ResponsePort.Choice());

            fault = (soap.Fault)get.ResponsePort;

            if (fault != null)
            {
                httpGet.ResponsePort.Post(new HttpResponseType(System.Net.HttpStatusCode.InternalServerError, fault));
            }
            else
            {
                httpGet.ResponsePort.Post(new HttpResponseType(System.Net.HttpStatusCode.OK, (MarkRobotState)get.ResponsePort));
            }

            yield break;
        }
Пример #10
0
 protected void UnhandledResponse(W3C.Soap.Fault fault)
 {
     _service.LogError("Unhandled fault response from partner service", fault);
     Decrement();
 }
Пример #11
0
 protected void FaultHandler(soap.Fault fault, string msg)
 {
     _service.LogError(null, msg, fault);
 }
        /// <summary>
        /// Retrieve the COM port specified in the SerialCOMService state.
        /// Perform initialization if COM port is correct and available.
        /// </summary>
        /// <returns>Enumerator of type ITask</returns>
        private IEnumerator <ITask> InternalInitialize()
        {
            // Make sure we have a valid and open COM port
            int currentCOMPort = 0;

            yield return(this.serialCOMServicePort.GetConfiguration().Choice(
                             s => currentCOMPort = s.PortNumber,
                             f => LogError("Failed to retrieve config from Serial Port partner service")));

            yield return(this.serialCOMServicePort.OpenPort().Choice(
                             s => LogInfo(string.Format("Opened COM{0}", currentCOMPort)),
                             f => LogError(string.Format("Failed to open COM{0}", currentCOMPort))));

            if (currentCOMPort == 0)
            {
                LogError("Parallax2011ReferencePlatformIoController Service failed to initialize: Check 'PortNumber' in serialcomservice.config.xml");
                this.Shutdown();
                yield break;
            }

            this.initialized = this.Initialize();
            if (this.initialized)
            {
                // Make sure we have a live Parallax board on the COM port by retrieving the FW version string
                serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
                sg.Timeout    = this.state.DefaultResponsePause;
                sg.Terminator = board.PacketTerminatorString;
                sg.Data       = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetVersionString));

                PortSet <serialcomservice.Packet, soap.Fault> resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                soap.Fault f = (soap.Fault)resultPort;
                if (f == null)
                {
                    serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
                    if (p != null)
                    {
                        if (p.Message != null)
                        {
                            string str = Encoding.ASCII.GetString(p.Message);
                            this.state.FWVersion = Convert.ToInt32(str, 16);
                        }
                    }
                }
                else
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                    LogError("Failed to receive FW version!");
                }

                LogInfo(string.Format("FW Version: {0}", this.state.FWVersion));

                serialcomservice.SendAndGetRequest accSendAndGet = new serialcomservice.SendAndGetRequest();

                accSendAndGet.Timeout    = this.state.DefaultResponsePause;
                accSendAndGet.Terminator = board.PacketTerminatorString;
                accSendAndGet.Data       = new serialcomservice.Packet(board.CreatePacket <ushort>(board.SetRampingValueString, (ushort)this.state.AccelerationRate));

                resultPort = this.serialCOMServicePort.SendAndGet(accSendAndGet);
                yield return(resultPort.Choice());

                f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(accSendAndGet.Data.Message)));
                    LogError("Failed to set acceleration!");
                }

                this.encoderTicksPerMeter = new double[]
                {
                    this.state.DriveState.LeftWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.LeftWheel.Radius * Math.PI),
                    this.state.DriveState.RightWheel.EncoderState.TicksPerRevolution / (2 * this.state.DriveState.RightWheel.Radius * Math.PI)
                };

                SaveState(this.state);
                base.Start();

                // Make sure the pin polling port is in the main interleave because it modifies service state
                MainPortInterleave.CombineWith(
                    new Interleave(
                        new ExclusiveReceiverGroup(
                            Arbiter.ReceiveWithIterator(true, this.pinPollingPort, this.PollPins)),
                        new ConcurrentReceiverGroup()));

                // Start the pin polling interval
                this.pinPollingPort.Post(DateTime.Now);
            }
        }
Пример #13
0
        /// <summary>
        /// Keep the FW watchdog alive by continuously retrieving pin and encoder values
        /// </summary>
        /// <param name="dt">A instance of type DateTime</param>
        /// <returns>A instance of IEnumerator of type ITask</returns>
        private IEnumerator <ITask> PollPins(DateTime dt)
        {
            try
            {
                serialcomservice.SendAndGetRequest sg = new serialcomservice.SendAndGetRequest();
                sg.Timeout    = this.state.DefaultResponsePause;
                sg.Terminator = board.PacketTerminatorString;
                sg.Data       = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetADCValuesString));
                var resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                soap.Fault f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                    yield break;
                }

                serialcomservice.Packet p = (serialcomservice.Packet)resultPort;
                if (this.HasFWError(p))
                {
                    LogError("Error returned reading ADC values from FW!");
                    yield break;
                }

                // Stash the analog pin values into a string
                string analogValues = Encoding.ASCII.GetString(p.Message);

                // Retrieve the digital pin values
                sg.Data    = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetDigitalValuesString));
                resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                    yield break;
                }

                p = (serialcomservice.Packet)resultPort;
                if (this.HasFWError(p))
                {
                    LogError("Error returned reading digital pin values from FW!");
                    yield break;
                }

                // Stash the digital pin values into a string
                string digitalValues = Encoding.ASCII.GetString(p.Message);

                // Perform all the parsing
                this.UpdatePinStatesFromFWString(analogValues, digitalValues);

                // Reset speed to 0 since it is currently unknown as opposed to having the last value be stuck
                this.state.DriveState.LeftWheel.WheelSpeed  = 0;
                this.state.DriveState.RightWheel.WheelSpeed = 0;

                // Retrieve both wheel speeds
                sg.Data    = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetCurrentSpeedString));
                resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                p = (serialcomservice.Packet)resultPort;
                if (this.HasFWError(p))
                {
                    LogError("Error received reading speed values from FW!");
                    yield break;
                }

                f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                    yield break;
                }

                this.UpdateSpeedStateFromFWString(Encoding.ASCII.GetString(p.Message));

                // Retrieve both wheel encoder values
                sg.Data    = new serialcomservice.Packet(board.CreatePacket <byte>(board.GetEncoderTicksString));
                resultPort = this.serialCOMServicePort.SendAndGet(sg);
                yield return(resultPort.Choice());

                f = (soap.Fault)resultPort;
                if (f != null)
                {
                    LogError(string.Format("Failed to send command: {0}", Encoding.ASCII.GetString(sg.Data.Message)));
                    yield break;
                }

                p = (serialcomservice.Packet)resultPort;
                if (this.HasFWError(p))
                {
                    LogError("Error received reading encoder values from FW!");
                    yield break;
                }

                // Reflect the returned bytes into encoder and motor states
                this.UpdateEncoderStatesFromFWString(Encoding.ASCII.GetString(p.Message));
            }
            finally
            {
                // Ensure we haven't been droppped
                if (ServicePhase == ServiceRuntimePhase.Started)
                {
                    // Issue another polling request
                    Activate(TimeoutPort(this.state.PinPollingInterval).Receive(this.pinPollingPort.Post));
                }
            }
        }