/// <summary>
/// Determines if a response packet contains an ERROR
/// </summary>
/// <param name="p">Packet received after a command</param>
/// <returns>True if error identifier detected, otherwise false</returns>
private bool HasFWError(serialcomservice.Packet p)
{
if (p == null || p.Message == null)
{
return(true);
}
if (Encoding.ASCII.GetString(p.Message).IndexOf(board.Error) != -1)
{
return(true);
}
return(false);
}
/// <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);
}
/// <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));
}
}
}
