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);
}
/// <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;
}
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();
}
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;
}
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);
}
/// <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);
}
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);
}
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);
}
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;
}
protected void UnhandledResponse(W3C.Soap.Fault fault)
{
_service.LogError("Unhandled fault response from partner service", fault);
Decrement();
}
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);
}
}
/// <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));
}
}
}
