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