/// <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>
/// Initializes this instance.
/// </summary>
/// <returns>Standard ccr iterator</returns>
private IEnumerator <ITask> Initialize()
{
var located = new PortSet <VisualEntity, Fault>();
SpawnIterator(located, this.LocateCameraEntity);
yield return(located.Choice());
var entity = (VisualEntity)located;
if (entity == null)
{
LogError("Kinect entity not found");
StartFailed();
yield break;
}
this.kinectEntity = (KinectEntity)entity;
if (this.state == null)
{
this.state = new kinect.KinectState
{
DepthImageFormat = DepthImageFormat.Resolution320x240Fps30,
FrameRate = MaxFramesPerSec,
IsDepthServiceUpdateEnabled = true,
IsWebCamServiceUpdateEnabled = true,
UseColor = true,
UseDepth = true,
UseSkeletalTracking = false,
ColorImageFormat = ColorImageFormat.RgbResolution640x480Fps30
};
SaveState(this.state);
}
this.panTiltState = InitialPanTiltState();
base.Start();
MainPortInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator(true, this.pollPort, this.DrainPendingRequests)),
new ConcurrentReceiverGroup()));
this.kinectOps.Post(new kinect.SetFrameRate(new kinect.SetFrameRateRequest(this.state.FrameRate)));
}
/// <summary>
/// Load files
/// </summary>
/// <param name="logFilesFolder">Root file directory</param>
internal void Load(string logFilesFolder)
{
LogFileResult loadResult;
if (this.serviceState != null && string.IsNullOrEmpty(logFilesFolder) == false)
{
loadResult = this.LoadLogs(logFilesFolder);
if (loadResult == LogFileResult.Success)
{
this.DisplayInfo(Resources.LogFilesLoaded);
this.RefreshUserInterface();
}
else if (loadResult == LogFileResult.XmlLogsDetected)
{
// need to prompt user where to save the new binary files
var newLogFolderPort = new PortSet <string, Fault>();
this.WpfInvoke(() => this.userInterface.PromptForNewLogFolder(newLogFolderPort));
Activate(newLogFolderPort.Choice(
newLogFilesFolder =>
{
if (string.IsNullOrEmpty(newLogFilesFolder) == false)
{
this.WpfInvoke(() => this.userInterface.ShowProgressBar());
var resultPort = ConvertXmlLogsToBinary(
logFilesFolder,
newLogFilesFolder,
progressUpdate => this.WpfInvoke(() => this.userInterface.UpdateProgress(progressUpdate)));
Activate(resultPort.Receive(
logFileResult =>
{
this.WpfInvoke(() => this.userInterface.HideProgressBar());
if (logFileResult == LogFileResult.Success)
{
Load(newLogFilesFolder);
}
}));
}
},
fault => { /*user canceled load*/ }));
}
}
}
void UpdateCameraImage(DateTime dt)
{
try
{
var resultPort = new PortSet <int[], Exception>();
_observer.CaptureScene(resultPort);
Activate(
resultPort.Choice(
(data) =>
{
var bmp = _observer.CreateBitmapFromArray(data);
WinFormsServicePort.Post(new FormInvoke(delegate() { _embeddedSimUI.SetCameraImage(bmp); }));
},
(ex) => { }));
}
finally
{
if (this.ServicePhase == ServiceRuntimePhase.Started)
{
Activate(TimeoutPort(50).Receive(UpdateCameraImage));
}
}
}
/// <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);
}
}
