/// <summary>
/// Redirects the HTTP request from an alternate contract to a partner
/// </summary>
/// <param name="contract">The contract.</param>
/// <param name="partner">The partner.</param>
/// <param name="context">The context.</param>
/// <param name="responsePort">The response port.</param>
private void RedirectHttpRequest(
string contract,
IPort partner,
HttpListenerContext context,
PortSet <HttpResponseType, Fault> responsePort)
{
var alternate = AlternateContractServiceInfo.Find(s => s.Contract == contract) ?? ServiceInfo;
var basePath = alternate.HttpServiceAlias.AbsolutePath;
var requestedPath = context.Request.Url.PathAndQuery;
var pathSuffix = requestedPath.Substring(basePath.Length);
var lookup = new DsspDefaultLookup();
partner.PostUnknownType(lookup);
this.Activate(
lookup.ResponsePort.Choice(
svcinfo =>
{
var redirectPath = svcinfo.HttpServiceAlias.AbsolutePath + pathSuffix;
context.Response.Redirect(redirectPath);
context.Response.Close();
responsePort.Post(new HttpResponseType());
},
responsePort.Post));
}
/// <summary>
/// Find sensors on the brick which can be added to
/// our contact sensor array.
/// </summary>
/// <returns></returns>
private IEnumerator <ITask> FindSensors()
{
#region Attach to the NXT Brick
LegoNxtConnection aggregate = new LegoNxtConnection(LegoNxtPort.Aggregation);
aggregate.PortOverride = "Contact Sensor Array";
brick.AttachRequest attachRequest = new brick.AttachRequest(
new brick.Registration(
aggregate,
LegoDeviceType.Aggregation,
"ContactSensorArray",
Contract.Identifier,
ServiceInfo.Service,
"ContactSensorArray"));
yield return(Arbiter.Choice(_legoBrickPort.AttachAndSubscribe(attachRequest, _legoBrickNotificationPort),
delegate(brick.AttachResponse rsp)
{
},
delegate(Fault fault)
{
LogError("Error in LEGO NXT Contact Sensor Array while attaching to brick", fault);
}));
#endregion
PortSet <brick.NxtBrickState, Fault> brickResponse = _legoBrickPort.Get();
yield return(Arbiter.Choice(
Arbiter.ReceiveWithIterator <brick.NxtBrickState>(false, brickResponse, ProcessBrickState),
Arbiter.Receive <Fault>(false, brickResponse, EmptyHandler <Fault>)));
yield break;
}
public static MultipleItemGather MultipleItemReceive <T0, T1>(PortSet <T0, T1> portSet, int totalItemCount, Handler <ICollection <T0>, ICollection <T1> > handler)
{
Handler <ICollection[]> handler2 = delegate(ICollection[] res)
{
List <T0> list = new List <T0>(res[0].Count);
List <T1> list2 = new List <T1>(res[1].Count);
IEnumerator enumerator = res[0].GetEnumerator();
while (enumerator.MoveNext())
{
T0 item = (T0)((object)enumerator.Current);
list.Add(item);
}
IEnumerator enumerator2 = res[1].GetEnumerator();
while (enumerator2.MoveNext())
{
T1 item2 = (T1)((object)enumerator2.Current);
list2.Add(item2);
}
handler(list, list2);
};
return(new MultipleItemGather(new Type[]
{
typeof(T0),
typeof(T1)
}, new IPortReceive[]
{
portSet.P0,
portSet.P1
}, totalItemCount, handler2));
}
/// <summary>
/// This method searches for a primary or alternate contract of the
/// service that is present in the contracts list. Requires a
/// taskQueue to activate tasks on. Throws NoContractFoundException
/// in a Fault if one cannot be found.
/// </summary>
/// <param name="taskQueue"></param>
/// <param name="service"></param>
/// <param name="contracts"></param>
/// <returns></returns>
public static PortSet <ServiceInfoType, Fault> FindCompatibleContract(DispatcherQueue taskQueue, Uri service, List <string> contracts)
{
PortSet <ServiceInfoType, Fault> returnPort = new PortSet <ServiceInfoType, Fault>();
PortSet <LookupResponse, Fault> responsePort = new PortSet <LookupResponse, Fault>();
//Console.WriteLine("RSUtils: Querying " + service);
DssEnvironment.ServiceForwarderUnknownType(service).PostUnknownType(
new DsspDefaultLookup()
{
Body = new LookupRequestType(), ResponsePort = responsePort
});
Arbiter.Activate(taskQueue, Arbiter.Choice <LookupResponse, Fault>(
responsePort,
delegate(LookupResponse resp)
{
try
{
//Console.WriteLine("RSUtils: Got response");
returnPort.Post(FindCompatibleContract(resp, contracts));
}
catch (NoContractFoundException e)
{
returnPort.Post(FaultOfException(e));
}
},
delegate(Fault failure)
{
returnPort.Post(failure);
}));
return(returnPort);
}
internal PortSet <string, Exception> DeserializeEnvelopeBody(loganalyzer.Envelope envelope)
{
var xmlResultPort = new PortSet <XmlWriter, Exception>();
var o = loganalyzer.DssLogAnalyzerHelper.DeserializeEnvelope(envelope).Body;
var stringbuilderOutput = new StringBuilder();
var xmlWriter = XmlWriter.Create(stringbuilderOutput, new XmlWriterSettings {
Indent = true
});
var ser = new SerializeToXmlWriter(o, xmlWriter, xmlResultPort);
SerializerPort.Post(ser);
var resultPort = new PortSet <string, Exception>();
Activate(
Arbiter.Choice(
xmlResultPort,
success => resultPort.Post(stringbuilderOutput.ToString()),
failure => resultPort.Post(failure)));
return(resultPort);
}
public PortSet CreateLevelPort(Vector2 _pos, string _objSetId, string _title = "port")
{
//Check if _objId has port 檢查該對應物件是否已紀錄
PortSet _portSet = portSets.Find(x => x.setGuid == _objSetId);
Debug.Log("3. CreateLevelPort port count" + portSets.Count);
if (_portSet != null)
{
Debug.Log("port esisted " + _portSet.setGuid + " tilie " + _title + " pos " + _pos);
Port[] ports = CreatePortSet(_pos, _title);
ports[0].name = _portSet.localInGuid;
ports[1].name = _portSet.localOutGuid;
_portSet.pointObjectName = _title;
//_portSet.position = _pos;
_portSet.position = new Vector2(Math.Abs(_pos.x), Math.Abs(_pos.y));
Debug.Log("port count " + outPorts.Count);
}
else
{
//else =>create new 否則創造一組
Port[] ports = CreatePortSet(_pos, _title);
//Save guid to name
_portSet = new PortSet(nodeGuid, _pos, _title);
ports[0].name = _portSet.localInGuid;
ports[1].name = _portSet.localOutGuid;
portSets.Add(_portSet);
Debug.Log("port created " + _portSet.setGuid);
}
return(_portSet);
}
public static Choice Choice <T0, T1>(this PortSet <T0, T1> portSet, Handler <T0> handler0, Handler <T1> handler1)
{
return(new Choice(new ReceiverTask[]
{
portSet.P0.Receive(handler0),
portSet.P1.Receive(handler1)
}));
}
/// <summary>
/// Initializes a new instance of the <see cref="SimpleBranchBlock"/> class.
/// </summary>
/// <param name="model">The model in which this object runs.</param>
/// <param name="name">The user-friendly name of this object. Typically not required to be unique in a pan-model context.</param>
/// <param name="guid">The GUID of this object. Typically registered as this object's ModelObject key, and thus, required to be unique in a pan-model context.</param>
public SimpleBranchBlock(IModel model, string name, Guid guid)
{
InitializeIdentity(model, name, null, guid);
m_portSet = new PortSet();
SetUpInputPort();
SetUpOutputPorts();
IMOHelper.RegisterWithModel(this);
}
public static Choice Choice <T0, T1>(PortSet <T0, T1> resultPort, Handler <T0> handler0, Handler <T1> handler1)
{
return(new Choice(new ReceiverTask[]
{
resultPort.P0.Receive(handler0),
resultPort.P1.Receive(handler1)
}));
}
private void ActionNotSupported(PortSet <DefaultUpdateResponseType, W3C.Soap.Fault> responsePort)
{
responsePort.Post(Fault.FromCodeSubcodeReason(
W3C.Soap.FaultCodes.Receiver,
DsspFaultCodes.ActionNotSupported,
Resources.NotModifiable
)
);
}
public MathSystem()
{
_iid = Config.GenerateInstanceId();
_inputs = new SignalSet();
_outputs = new SignalSet();
_allSignals = new SignalSet();
_allBuses = new BusSet();
_allPorts = new PortSet();
}
/// <summary>
/// Add an iRobot command to our script.
/// </summary>
/// <param name="cmd"></param>
/// <param name="responsePort"></param>
private void AddScriptCommand(Roomba.RoombaCommand cmd, PortSet <DefaultInsertResponseType, Fault> responsePort)
{
_state.CurrentScript.Commands.Add(cmd.ToString());
_state.CurrentScript.PacketData = Roomba.ByteArray.Combine(_state.CurrentScript.PacketData, cmd.GetPacket());
_state.CurrentScript.PacketData[0] = (byte)(_state.CurrentScript.PacketData.Length - 1);
_state.CurrentScript.ExpectedScriptResponseBytes += cmd.ExpectedResponseBytes();
responsePort.Post(DefaultInsertResponseType.Instance);
}
public ConditionalCollectVisitor(Predicate <Signal> signalMatch, Predicate <Port> portMatch, Predicate <Bus> busMatch)
{
_signals = new SignalSet();
_ports = new PortSet();
_buses = new BusSet();
_signalMatch = signalMatch;
_portMatch = portMatch;
_busMatch = busMatch;
}
public ConditionalCollectVisitor(Predicate<Signal> signalMatch, Predicate<Port> portMatch, Predicate<Bus> busMatch)
{
_signals = new SignalSet();
_ports = new PortSet();
_buses = new BusSet();
_signalMatch = signalMatch;
_portMatch = portMatch;
_busMatch = busMatch;
}
public ConditionalCollectVisitor()
{
_signals = new SignalSet();
_ports = new PortSet();
_buses = new BusSet();
_signalMatch = DummySignalPredicate;
_portMatch = DummyPortPredicate;
_busMatch = DummyBusPredicate;
}
public ConditionalCollectVisitor()
{
_signals = new SignalSet();
_ports = new PortSet();
_buses = new BusSet();
_signalMatch = DummySignalPredicate;
_portMatch = DummyPortPredicate;
_busMatch = DummyBusPredicate;
}
public MathSystem()
{
_iid = Config.GenerateInstanceId();
_inputs = new SignalSet();
_outputs = new SignalSet();
_allSignals = new SignalSet();
_allBuses = new BusSet();
_allPorts = new PortSet();
}
public Nexus(IModel model, string name, Guid guid, IPortSelector portSelector)
{
InitializeIdentity(model, name, null, guid);
m_ports = new PortSet();
m_portSelector = portSelector;
m_cantTakeOrPeekFromNexus = new DataProvisionHandler(CantTakeOrPeekFromNexus);
m_canAlwaysAcceptData = new DataArrivalHandler(OnDataArrived);
IMOHelper.RegisterWithModel(this);
}
public PortSet GetLinkedPort(string _outPortGuid)
{
LinkData _link = linkDatas.Find(x => x.outPortGuid == _outPortGuid);
if (_link == null)
{
return(null);
}
PortSet _inPortSet = allPortSets.Find(x => x.localInGuid == _link.inPortGuid);
return(_inPortSet);
}
protected override IEnumerator <ITask> GetCallback(Myro.Services.Generic.Vector.GetRequestInfo request, PortSet <Myro.Services.Generic.Vector.CallbackResponseType, Fault> responsePort)
{
if (request is vector.GetAllRequestInfo)
{
PortSet <DefaultQueryResponseType, Fault> responses = new PortSet <DefaultQueryResponseType, Fault>();
for (int i = 0; i < _state.Values.Count; i++)
{
int myI = i;
Activate(Arbiter.Choice(_scribblerPort.GetObstacle(i),
delegate(brick.UInt16Body r)
{
_state.Set(myI, RSUtils.NormalizeUShort(r.Value), DateTime.Now);
responses.Post(DefaultQueryResponseType.Instance);
},
delegate(Fault f) { responses.Post(f); }));
}
yield return(Arbiter.MultipleItemReceive(responses, _state.Values.Count,
delegate(ICollection <DefaultQueryResponseType> ss, ICollection <Fault> fs)
{
if (fs.Count == 0)
{
responsePort.Post(vector.CallbackResponseType.Instance);
}
else
{
var reasons = new List <ReasonText>();
foreach (var f in fs)
{
reasons.AddRange(f.Reason.AsEnumerable());
}
responsePort.Post(new Fault()
{
Detail = new Detail()
{
Any = fs.ToArray()
}, Reason = reasons.ToArray()
});
}
}));
}
else
{
var info = (vector.GetElementRequestInfo)request;
yield return(Arbiter.Choice(_scribblerPort.GetObstacle(info.Index),
delegate(brick.UInt16Body r)
{
_state.Set(info.Index, RSUtils.NormalizeUShort(r.Value), DateTime.Now);
responsePort.Post(vector.CallbackResponseType.Instance);
},
delegate(Fault f) { responsePort.Post(f); }));
}
yield break;
}
/// <summary>
/// Check the hardware value 5 times per second and send a notification if it has changed.
/// </summary>
/// <param name="timeout"></param>
/// <returns></returns>
private IEnumerator <ITask> CheckForStateChange(DateTime timeout)
{
while (true)
{
if (_entity != null)
{
// get the image from the CameraEntity
PortSet <int[], Exception> result = new PortSet <int[], Exception>();
_entity.CaptureScene(result);
double averageR = 0, averageG = 0, averageB = 0;
yield return(Arbiter.Choice(result,
delegate(int[] data)
{
for (int i = 0; i < data.Length; i++)
{
int c = data[i];
int r = (0x00FF0000 & c) >> 16;
int g = (0x0000FF00 & c) >> 8;
int b = (0x000000FF & c);
averageR += r;
averageG += g;
averageB += b;
}
// calculate the average brightness, scale it to [0-1] range
averageR = averageR / (255.0 * data.Length);
averageG = averageG / (255.0 * data.Length);
averageB = averageB / (255.0 * data.Length);
},
delegate(Exception ex)
{
LogError(ex);
}
));
double SENSOR_TOLERANCE = 0.005;
if (Math.Abs(averageR - _state.NormalizedAverageRed) > SENSOR_TOLERANCE ||
Math.Abs(averageG - _state.NormalizedAverageGreen) > SENSOR_TOLERANCE ||
Math.Abs(averageB - _state.NormalizedAverageBlue) > SENSOR_TOLERANCE)
{
// send notification of state change
_state.NormalizedAverageRed = averageR;
_state.NormalizedAverageGreen = averageG;
_state.NormalizedAverageBlue = averageB;
_state.TimeStamp = DateTime.Now;
base.SendNotification <Replace>(_submgrPort, _state);
}
}
yield return(Arbiter.Receive(false, TimeoutPort(200), delegate { }));
}
}
private void SetUpPort()
{
//clear port
if (portSet == null)
{
portSet = new PortSet(nodeGuid, Vector2.zero, "Start");
}
Port outputPort = AddOutputPort("Start", Port.Capacity.Single);
portSet.nodeGuid = nodeGuid;
outputPort.name = portSet.localOutGuid;
outputContainer.Add(outputPort);
}
public IEnumerator <ITask> SetDrivePowerHandler(drive.SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet <DefaultUpdateResponseType, Fault> responsePort = new PortSet <DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination();
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.LeftWheelPower
});
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest()
{
TargetPower = setDrivePower.Body.RightWheelPower
});
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
Update update = new Update(_state);
SendNotification <Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive <DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection <DefaultUpdateResponseType> successList, ICollection <Fault> failureList)
{
if (successList.Count == 2)
{
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
}
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
bool HasError <T>(PortSet <T, Fault> sensorOrFault)
{
Fault fault = (Fault)sensorOrFault;
if (fault != null)
{
LogError(fault.ToException());
return(true);
}
else
{
return(false);
}
}
public virtual IEnumerator <ITask> ConfirmHandler(Confirm confirm)
{
PortSet <bool, Exception> result = new PortSet <bool, Exception>();
ConfirmForm form = null;
RunForm runForm = new RunForm(
delegate()
{
form = new ConfirmForm(result);
form.Message = confirm.Body.Message;
form.Countdown = _defaultTimeout;
return(form);
}
);
WinFormsServicePort.Post(runForm);
yield return(Arbiter.Choice(
runForm.pResult,
delegate(SuccessResult success) { },
delegate(Exception e)
{
result.Post(e);
}
));
yield return(Arbiter.Choice(
result,
delegate(bool confirmed)
{
ConfirmResponse response = new ConfirmResponse();
response.Confirmed = confirmed;
confirm.ResponsePort.Post(response);
if (form.Timeout)
{
LogWarning("Confirm dialog cancelled due to timeout.");
}
},
delegate(Exception e)
{
Fault fault = Fault.FromException(e);
LogError(null, "Error in Confirm Handler", fault);
confirm.ResponsePort.Post(fault);
}
));
}
public MathSystem(Context context)
{
if (context == null)
{
throw new ArgumentNullException("context");
}
_context = context;
_iid = _context.GenerateInstanceId();
_inputs = new SignalSet();
_outputs = new SignalSet();
_allSignals = new SignalSet();
_allBuses = new BusSet();
_allPorts = new PortSet();
}
/// <summary>
/// Prompt for New log folder
/// </summary>
/// <param name="resultPort">The result port to post result</param>
internal void PromptForNewLogFolder(PortSet <string, Fault> resultPort)
{
if (MessageBox.Show(
Properties.Resources.XmlToBinaryConversionWarning,
Properties.Resources.XmlToBinaryConversionWarningCaption,
MessageBoxButton.YesNo,
MessageBoxImage.Warning) == MessageBoxResult.Yes)
{
resultPort.Post(this.PresentLoadFileDialog(Properties.Resources.SelectFolderForConvertedBinaryFiles));
}
else
{
resultPort.Post(Fault.FromException(new Exception(Properties.Resources.UserCanceledLoad)));
}
}
/// <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)));
}
public IEnumerator <ITask> DropHandler(DsspDefaultDrop drop)
{
// Update our state, but only wait for one second.
PortSet <DefaultUpdateResponseType, Fault> responsePort = UpdateServiceStatus(LegoNxtBumperStatus.ShuttingDown);
yield return(Arbiter.Choice(
Arbiter.Receive <DefaultUpdateResponseType>(false, responsePort,
delegate(DefaultUpdateResponseType response) { }),
Arbiter.Receive <Fault>(false, responsePort,
delegate(Fault fault) { }),
Arbiter.Receive <DateTime>(false, TimeoutPort(1000),
delegate(DateTime timeout) { })));
base.DefaultDropHandler(drop);
yield break;
}
public Splitter(IModel model, string name, Guid guid, int nOuts)
{
IMOHelper.Initialize(ref m_model, model, ref m_name, name, ref m_description, null, ref m_guid, guid);
m_ports = new PortSet();
m_input = new SimpleInputPort(model, "Input", Guid.NewGuid(), this, GetDataArrivalHandler());
//AddPort(m_input); <-- Done in SIP's ctor.
Input = m_input;
Outputs = new IOutputPort[nOuts];
m_outputs = new SimpleOutputPort[nOuts];
for (int i = 0; i < nOuts; i++)
{
m_outputs[i] = new SimpleOutputPort(model, "Output" + i, Guid.NewGuid(), this, GetDataProvisionHandler(i), GetPeekHandler(i));
Outputs[i] = m_outputs[i];
//AddPort(m_outputs[i]); <-- Done in SOP's ctor.
}
IMOHelper.RegisterWithModel(this);
}
/// <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*/ }));
}
}
}
private IEnumerator <ITask> PlaySound(bool synchronous, PortSet <DefaultSubmitResponseType, Fault> responsePort, SystemSound systemSound)
{
if (synchronous)
{
systemSound.Play();
}
else
{
Spawn(delegate()
{
systemSound.Play();
}
);
}
responsePort.Post(DefaultSubmitResponseType.Instance);
yield break;
}
protected override IEnumerator<ITask> GetCallback(Myro.Services.Generic.Vector.GetRequestInfo request, PortSet<Myro.Services.Generic.Vector.CallbackResponseType, Fault> responsePort)
{
if (request is vector.GetAllRequestInfo)
{
PortSet<DefaultQueryResponseType, Fault> responses = new PortSet<DefaultQueryResponseType, Fault>();
for (int i = 0; i < _state.Values.Count; i++)
{
int myI = i;
Activate(Arbiter.Choice(_scribblerPort.GetObstacle(i),
delegate(brick.UInt16Body r)
{
//_state.Set(myI, RSUtils.NormalizeUShort(r.Value), DateTime.Now);
_state.Set(myI, r.Value, DateTime.Now);
responses.Post(DefaultQueryResponseType.Instance);
},
delegate(Fault f) { responses.Post(f); }));
}
yield return Arbiter.MultipleItemReceive(responses, _state.Values.Count,
delegate(ICollection<DefaultQueryResponseType> ss, ICollection<Fault> fs)
{
if (fs.Count == 0)
responsePort.Post(vector.CallbackResponseType.Instance);
else
{
responsePort.Post(fs.First());
//// f.Reason was sometimes null
//var reasons = new List<ReasonText>();
//foreach (var f in fs)
// reasons.AddRange(f.Reason.AsEnumerable());
//responsePort.Post(new Fault() { Detail = new Detail() { Any = fs.ToArray() }, Reason = reasons.ToArray() });
}
});
}
else
{
var info = (vector.GetElementRequestInfo)request;
yield return Arbiter.Choice(_scribblerPort.GetObstacle(info.Index),
delegate(brick.UInt16Body r)
{
_state.Set(info.Index, RSUtils.NormalizeUShort(r.Value), DateTime.Now);
responsePort.Post(vector.CallbackResponseType.Instance);
},
delegate(Fault f) { responsePort.Post(f); });
}
yield break;
}
/// <summary>
/// Actuator callback
/// </summary>
protected override IEnumerator<ITask> SetCallback(Myro.Services.Generic.Vector.SetRequestInfo request, PortSet<vector.CallbackResponseType, Fault> responsePort)
{
var req = request as vector.SetElementsRequestInfo;
if (req != null)
{
bool play = false;
bool loud = false;
foreach (var i in req.Indices)
if (i == 0 || i == 1 || i == 2)
play = true;
else if (i == 3)
loud = true;
Fault error = null;
if (loud)
yield return Arbiter.Choice(setLoud(),
delegate(vector.CallbackResponseType s) { },
delegate(Fault f) { error = f; });
if (error == null && play)
yield return Arbiter.Choice(playTone(),
delegate(vector.CallbackResponseType s1) { },
delegate(Fault f) { error = f; });
if (error == null)
responsePort.Post(vector.CallbackResponseType.Instance);
else
responsePort.Post(error);
}
else
{
// Otherwise it was a SetAllRequestInfo
Activate(Arbiter.Choice(setLoud(),
delegate(vector.CallbackResponseType s)
{
Activate(Arbiter.Choice(playTone(),
delegate(vector.CallbackResponseType s1) { responsePort.Post(vector.CallbackResponseType.Instance); },
delegate(Fault f) { responsePort.Post(f); }));
},
delegate(Fault f) { responsePort.Post(f); }));
}
yield break;
}
/// <summary>
/// Constructor for the class.
/// </summary>
/// <param name="portName">The port for communication</param>
/// <param name="queue">The robotics runtime DispatcherQueue</param>
public MotorControl(string portName, DispatcherQueue queue, GenericMotorService b)
{
myMotors = new SerialPort();
state = 0;
receive_ack = "";
dq = queue;
motorAckRecieve = new
PortSet<SendAck, Stop, Turn, SetSpeed, StopComplete, TurnComplete, BumperActivated>();
bumper = b;
//Initialize Serial Port Parameters
myMotors.PortName = portName;
myMotors.BaudRate = DEFAULT_BAUD_RATE;
myMotors.Parity = DEFAULT_PARITY;
myMotors.DataBits = DEFAULT_DATA_BITS;
myMotors.StopBits = DEFAULT_STOP_BITS;
myMotors.Handshake = DEFAULT_HANDSHAKE;
myMotors.ReadTimeout = DEFAULT_TIMEOUT;
myMotors.WriteTimeout = DEFAULT_TIMEOUT;
}
private bool ValidateDriveEnabledAndRespondHelper(PortSet<DefaultUpdateResponseType, Fault> responsePort)
{
Fault fault = null;
// Acknowledge request or fault
if (_genericState.IsEnabled == false)
{
fault = Fault.FromException(new InvalidOperationException("Attempting to process a drive operation, but the differential drive is not enabled"));
responsePort.Post(fault);
}
else
{
responsePort.Post(DefaultUpdateResponseType.Instance);
}
return _genericState.IsEnabled;
}
public CollectVisitor()
{
_signals = new SignalSet();
_ports = new PortSet();
_buses = new BusSet();
}
public IEnumerator<ITask> SetDrivePowerHandler(SetDrivePower setDrivePower)
{
ValidateDriveConfiguration(false);
_state.TimeStamp = DateTime.Now;
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
// Add a coordination header to our motor requests
// so that advanced motor implementations can
// coordinate the individual motor reqests.
coord.ActuatorCoordination coordination = new coord.ActuatorCoordination(2);
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(setDrivePower.Body.LeftWheelPower);
leftPower.ResponsePort = responsePort;
leftPower.AddHeader(coordination);
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(setDrivePower.Body.RightWheelPower);
rightPower.ResponsePort = responsePort;
rightPower.AddHeader(coordination);
_rightMotorPort.Post(rightPower);
// send notification to subscription manager
drive.Update update = new drive.Update(_state);
SendNotification<drive.Update>(_subMgrPort, update);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
if (successList.Count == 2)
setDrivePower.ResponsePort.Post(new DefaultUpdateResponseType());
foreach (Fault fault in failureList)
{
setDrivePower.ResponsePort.Post(fault);
break;
}
}));
yield break;
}
/// <summary>
/// Rotate the the drive (positive degrees turn counterclockwise)
/// </summary>
/// <param name="degrees">(positive degrees turn counterclockwise)</param>
/// <param name="power">(-1.0 to 1.0)</param>
IEnumerator<ITask> RotateUntilDegrees(double degrees, double power)
{
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return (Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return (Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * 3.14159 / 360;
//compute tick to stop at
int stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
int stopRightWheelAt = (int)Math.Round(arcDistance / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
// Subscribe to the encoders on internal ports
encoder.EncoderOperations leftNotificationPort = new encoder.EncoderOperations();
encoder.EncoderOperations rightNotificationPort = new encoder.EncoderOperations();
encoder.Subscribe op = new encoder.Subscribe();
op.Body = new SubscribeRequestType();
op.NotificationPort = leftNotificationPort;
_leftEncoderPort.Post(op);
yield return (Arbiter.Choice(op.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, leftNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(leftNotificationPort, update, _leftMotorPort, stopLeftWheelAt);
}));
},
delegate(Fault fault) { LogError(fault); }
));
encoder.Subscribe op2 = new encoder.Subscribe();
op2.Body = new SubscribeRequestType();
op2.NotificationPort = rightNotificationPort;
_leftEncoderPort.Post(op2);
yield return (Arbiter.Choice(op2.ResponsePort,
delegate(SubscribeResponseType response)
{
//subscription was successful, start listening for encoder replace messages
Activate(Arbiter.Receive<encoder.UpdateTickCount>(false, rightNotificationPort,
delegate(encoder.UpdateTickCount update)
{
StopMotorWithEncoderHandler(rightNotificationPort, update, _rightMotorPort, stopRightWheelAt);
}
));
},
delegate(Fault fault) { LogError(fault); }
));
//start moving
// start rotate operation
_state.RotateDegreesStage = DriveStage.Started;
RotateDegrees rotateUpdate = new RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = DriveStage.Started;
_internalDriveOperationsPort.Post(rotateUpdate);
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
double rightPow;
double leftPow;
if (degrees > 0)
{
rightPow = power;
leftPow = -power;
}
else
{
rightPow = -power;
leftPow = power;
}
Motor.SetMotorPower leftPower = new Motor.SetMotorPower(leftPow);
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
Motor.SetMotorPower rightPower = new Motor.SetMotorPower(rightPow);
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
_state.RotateDegreesStage = DriveStage.Completed;
//complete
rotateUpdate.Body.RotateDegreesStage = DriveStage.Completed;
_internalDriveOperationsPort.Post(rotateUpdate);
}
/// <summary>
/// Handles http requests
/// </summary>
/// <param name="context">Http context</param>
/// <param name="responsePort">Response port</param>
/// <returns>CCR ITask enumerator</returns>
private IEnumerator<ITask> HttpHandler(HttpListenerContext context, PortSet<HttpResponseType, Fault> responsePort)
{
HttpListenerRequest request = context.Request;
HttpListenerResponse response = context.Response;
string type = "image/png";
ImageFormat format = ImageFormat.Png;
if (request.Url.Query.ToLowerInvariant().EndsWith("depth"))
{
Bitmap image = null;
if (!this.TryCreateDepthImage(out image))
{
responsePort.Post(Fault.FromException(new InvalidOperationException()));
yield break;
}
using (image)
{
using (var memStream = new MemoryStream())
{
image.Save(memStream, ImageFormat.Png);
response.AddHeader("Cache-Control", "No-cache");
response.ContentType = type;
memStream.WriteTo(response.OutputStream);
}
}
Microsoft.Dss.Core.DsspHttpUtilities.Utilities.HttpClose(context);
}
else if (request.Url.Query.ToLowerInvariant().EndsWith("floor"))
{
// show the pre-calculated floor/ceiling map
if (this.floorCeilingMaxDepths == null)
{
responsePort.Post(Fault.FromException(new InvalidOperationException()));
yield break;
}
// insert the depth image bits
var imageBits = new byte[DefaultDepthCamImageWidth * DefaultDepthcamImageHeight * 3];
for (int i = 0; i < this.depthDownSampleBuffer.Length; i++)
{
var depth8bpp = (this.floorCeilingMaxDepths[i] * 255) / (int)MaxValidDepth;
imageBits[(i * 3) + 0] = (byte)depth8bpp;
imageBits[(i * 3) + 1] = (byte)depth8bpp;
imageBits[(i * 3) + 2] = (byte)depth8bpp;
}
using (var image = new Bitmap(DefaultDepthCamImageWidth, DefaultDepthcamImageHeight, PixelFormat.Format24bppRgb))
{
var raw = image.LockBits(
new Rectangle(0, 0, DefaultDepthCamImageWidth, DefaultDepthcamImageHeight),
ImageLockMode.WriteOnly,
PixelFormat.Format24bppRgb);
System.Runtime.InteropServices.Marshal.Copy(imageBits, 0, raw.Scan0, imageBits.Length);
image.UnlockBits(raw);
using (var memStream = new MemoryStream())
{
image.Save(memStream, ImageFormat.Png);
response.AddHeader("Cache-Control", "No-cache");
response.ContentType = type;
memStream.WriteTo(response.OutputStream);
}
}
Microsoft.Dss.Core.DsspHttpUtilities.Utilities.HttpClose(context);
}
else
{
responsePort.Post(Fault.FromException(new NotSupportedException()));
yield break;
}
response.Close();
}
public void ReceiveFromPortSetUnderSharedMode()
{
var ps = new PortSet (typeof (int), typeof(string));
var dq = new SerialDispatchQueue ();
ps.Mode = PortSetMode.SharedPort;
try {
Arbiter.ReceiveFromPortSet (true, ps, (int a)=> { });
Assert.Fail ("#1");
} catch (InvalidOperationException) {}
}
public virtual IEnumerator<ITask> SendFileHandler(CopyFileToBrick sendFile)
{
Fault faultResponse = null;
bool fileExists = false;
bool done = false;
#region See if the file is already on the LEGO NXT.
nxtcmd.LegoCommand cmd = new nxtcmd.LegoFindFirst(sendFile.Body.FileName);
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
delegate(nxtcmd.LegoResponse ok)
{
nxtcmd.LegoResponseFindFirst ffResponse = nxtcmd.LegoResponse.Upcast<nxtcmd.LegoResponseFindFirst>(ok);
if (ffResponse.Success && (ffResponse.FileName == sendFile.Body.FileName))
{
fileExists = true;
bool fileMatches = (sendFile.Body.FileData != null && sendFile.Body.FileData.Length == ffResponse.FileSize);
if (!sendFile.Body.ReplaceExistingFile)
{
string msg = ((fileMatches) ? "A matching file" : "A different file with the same name") + " already exists on the LEGO NXT Brick.";
sendFile.ResponsePort.Post(Fault.FromException(new System.IO.IOException(msg)));
done = true;
}
}
},
delegate(Fault fault)
{
sendFile.ResponsePort.Post(fault);
done = true;
});
#endregion
if (done)
yield break;
if (fileExists)
{
#region Remove the existing file
cmd = new nxtcmd.LegoDelete(sendFile.Body.FileName);
yield return Arbiter.Choice(_legoBrickPort.SendNxtCommand(cmd),
EmptyHandler<nxtcmd.LegoResponse>,
delegate(Fault fault)
{
done = true;
sendFile.ResponsePort.Post(fault);
});
#endregion
}
if (done)
yield break;
#region Upload the file to the Brick.
byte[] fileData = sendFile.Body.FileData;
string fileName = sendFile.Body.FileName;
if (fileData != null)
{
int pgmLength = fileData.Length;
int handle = -1;
LogInfo(LogGroups.Console, "Downloading " + fileName + " to LEGO NXT.");
PortSet<nxtcmd.LegoResponse, Fault> responsePort = new PortSet<nxtcmd.LegoResponse, Fault>();
done = false;
int trial = 0;
while (trial < 5 && !done)
{
nxtcmd.LegoOpenWriteLinear openWrite = new nxtcmd.LegoOpenWriteLinear(fileName, pgmLength);
responsePort = _legoBrickPort.SendNxtCommand(openWrite);
yield return Arbiter.Choice(
Arbiter.Receive<nxtcmd.LegoResponse>(false, responsePort,
delegate(nxtcmd.LegoResponse response)
{
nxtcmd.LegoResponseOpenWriteLinear responseOpenWriteLinear = new nxtcmd.LegoResponseOpenWriteLinear(response.CommandData);
if (response.ErrorCode == LegoErrorCode.Success || response.ErrorCode == LegoErrorCode.FileExists)
{
handle = responseOpenWriteLinear.Handle;
done = true;
}
else if (response.ErrorCode == LegoErrorCode.NoSpace)
{
faultResponse = Fault.FromException(new System.IO.IOException("Out of space on LEGO NXT Brick.\nPlease remove one or more LEGO NXT programs on the NXT Brick."));
trial = 999;
}
else
{
faultResponse = Fault.FromException(new System.IO.IOException("Error preparing to upload " + fileName + " file to the LEGO NXT: " + response.ErrorCode.ToString()));
}
}),
Arbiter.Receive<Fault>(false, responsePort,
delegate(Fault fault)
{
faultResponse = fault;
}));
trial++;
}
if (!done)
{
if (faultResponse == null)
faultResponse = Fault.FromException(new System.IO.IOException("Failed to create the new file"));
sendFile.ResponsePort.Post(faultResponse);
yield break;
}
done = false;
trial = 0;
while (trial < 5 && !done)
{
nxtcmd.LegoWrite legoWrite = new nxtcmd.LegoWrite(handle, fileData);
responsePort = _legoBrickPort.SendNxtCommand(legoWrite);
yield return Arbiter.Choice(
Arbiter.Receive<nxtcmd.LegoResponse>(false, responsePort,
delegate(nxtcmd.LegoResponse response)
{
nxtcmd.LegoResponseWrite responseWrite = new nxtcmd.LegoResponseWrite(response.CommandData);
if (response.ErrorCode == LegoErrorCode.Success)
{
if (pgmLength != responseWrite.BytesWritten)
LogWarning(LogGroups.Console, "Warning: " + fileName + " file length on LEGO NXT does not match the PC.");
done = true;
}
else
{
faultResponse = Fault.FromException(new System.IO.IOException("Error sending " + fileName + " file to the LEGO NXT: " + response.ErrorCode.ToString()));
LogError(faultResponse);
}
}),
Arbiter.Receive<Fault>(false, responsePort,
delegate(Fault fault)
{
faultResponse = fault;
LogError(LogGroups.Console, "Timed out sending " + fileName + " file to the LEGO NXT");
}));
trial++;
}
if (!done)
{
if (faultResponse == null)
faultResponse = Fault.FromException(new System.IO.IOException("Failed to write to the new file,"));
sendFile.ResponsePort.Post(faultResponse);
yield break;
}
// Now Close the Write Buffer.
done = false;
nxtcmd.LegoClose legoClose = new nxtcmd.LegoClose(handle);
legoClose.TryCount = 5;
responsePort = _legoBrickPort.SendNxtCommand(legoClose);
yield return Arbiter.Choice(
Arbiter.Receive<nxtcmd.LegoResponse>(false, responsePort,
delegate(nxtcmd.LegoResponse response)
{
if (response.ErrorCode == LegoErrorCode.Success)
{
sendFile.ResponsePort.Post(DefaultSubmitResponseType.Instance);
done = true;
}
else
{
faultResponse = Fault.FromException(new System.IO.IOException("Error closing " + fileName + " file on the LEGO NXT: " + response.ErrorCode.ToString()));
LogError(faultResponse);
}
}),
Arbiter.Receive<Fault>(false, responsePort,
delegate(Fault fault)
{
faultResponse = fault;
LogError(LogGroups.Console, "Timed out closing file during SendFile.");
}));
}
else
{
faultResponse = Fault.FromException(new ArgumentNullException("The source file was not provided"));
}
if (done)
yield break;
if (faultResponse == null)
faultResponse = Fault.FromException(new System.IO.IOException("Failed to write to the new file,"));
sendFile.ResponsePort.Post(faultResponse);
#endregion
yield break;
}
private void ActionNotSupported(PortSet<DefaultUpdateResponseType, W3C.Soap.Fault> responsePort)
{
responsePort.Post(Fault.FromCodeSubcodeReason(
W3C.Soap.FaultCodes.Receiver,
DsspFaultCodes.ActionNotSupported,
Resources.NotModifiable
)
);
}
public void ReceiveFromPortSet()
{
var ps = new PortSet (typeof (int), typeof(string));
var dq = new SerialDispatchQueue ();
int cnt = 1;
var task = Arbiter.ReceiveFromPortSet (true, ps, (int a)=> { cnt += a; });
task.TaskQueue = dq;
task.Execute ();
Assert.AreEqual (1, ((IPortReceive)ps [typeof (int)]).GetReceivers().Length, "#1");
ps [typeof (int)].PostUnknownType (10);
Assert.AreEqual (11, cnt, "#2");
}
/// <summary>
/// Service Initialization
/// Connect to the Brick and then open up the service for all requests
/// </summary>
/// <returns></returns>
public virtual IEnumerator<ITask> ServiceInitialization(CreateResponse createResponse)
{
// Set up the _brickPort to our communications service
_brickPort = ServiceForwarder<comm.LegoCommOperations>(createResponse.Service);
// Subscribe to the communications port.
yield return Arbiter.Choice(_brickPort.Subscribe(_brickNotificationPort),
EmptyHandler<SubscribeResponseType>,
EmptyHandler<Fault>);
// If the SerialPort is not set, start the service,
// but do not attempt to connect to the actual hardware.
bool done = (_state.Configuration.SerialPort == 0);
if (!done)
{
// If we are not done yet, attempt to connect to the hardware.
NxtBrickOperations _initPort = new NxtBrickOperations();
PortSet<DefaultUpdateResponseType, Fault> _initResponse = new PortSet<DefaultUpdateResponseType, Fault>();
ConnectToHardware connectToHardware = new ConnectToHardware(_state.Configuration, _initResponse);
_initPort.Post(connectToHardware);
// Special one time handler to connect to the hardware before we open up the service to receive messages.
Activate<ITask>(
Arbiter.ReceiveWithIterator<ConnectToHardware>(false, _initPort, ConnectToHardwareHandler),
new Interleave(
new TeardownReceiverGroup(
Arbiter.ReceiveWithIterator<DefaultUpdateResponseType>(false, _initResponse, InitializationComplete),
Arbiter.ReceiveWithIterator<Fault>(false, _initResponse, InitializationComplete),
Arbiter.ReceiveWithIterator<DsspDefaultDrop>(false, _internalMainPort, DropHandler)
),
new ExclusiveReceiverGroup(
Arbiter.ReceiveWithIterator<AttachAndSubscribe>(true, _internalMainPort, AttachAndSubscribeHandler),
Arbiter.ReceiveWithIterator<ReserveDevicePort>(true, _internalMainPort, ReserveDevicePortHandler),
Arbiter.ReceiveWithIterator<Subscribe>(true, _internalMainPort, SubscribeHandler),
Arbiter.ReceiveWithIterator<Detach>(true, _internalMainPort, DetachHandler),
Arbiter.ReceiveWithIterator<AdjustPollingFrequency>(true, _internalMainPort, AdjustPollingFrequencyHandler)
),
new ConcurrentReceiverGroup())
);
}
else
{
SpawnIterator<DefaultUpdateResponseType>(DefaultUpdateResponseType.Instance, InitializationComplete);
}
yield break;
}
public static void FindAll(IEnumerable<Signal> rootSignals, bool ignoreHold, out SignalSet signals, out PortSet ports)
{
CollectVisitor visitor = new CollectVisitor();
AllSpanningTreeStrategy.Instance.Traverse(rootSignals, visitor, ignoreHold);
signals = visitor.Signals;
ports = visitor.Ports;
}
/// <summary>
/// drives a specified number of meters
/// </summary>
IEnumerator<ITask> DriveUntilDistance(double distance, double power)
{
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance(distance=" + distance + " meters, power=" + power + ")");
#endif
EncoderTicksEnabled = false;
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return (Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return (Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(Math.Abs(distance) / (2.0 * 3.14159 * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//start moving
double Pow;
if (distance > 0)
Pow = power;
else
Pow = -power;
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
// send notification of driveDistance start to subscription manager
_state.DriveDistanceStage = drive.DriveStage.Started;
drive.DriveDistance driveUpdate = new drive.DriveDistance();
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Started");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = Pow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() start moving: distance=" + distance + " meters");
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
#endif
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: DriveUntilDistance() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
// send notification of driveDistance complete to subscription manager
driveUpdate.Body.DriveDistanceStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: DriveUntilDistance() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(driveUpdate);
_state.DriveDistanceStage = drive.DriveStage.Completed;
}
/// <summary>
/// Send a success or failure SetMotorPower response.
/// </summary>
/// <param name="responsePort"></param>
/// <param name="success"></param>
private void RespondToMotorPowerRequest(PortSet<DefaultUpdateResponseType, Fault> responsePort, bool success)
{
if (success)
{
LogVerbose(LogGroups.Console, "SetMotorPower Completed Successfully");
responsePort.Post(DefaultUpdateResponseType.Instance);
}
else
{
string msg = "The motor operation was cancelled by a newer motor request.";
responsePort.Post(Fault.FromException(new OperationCanceledException(msg)));
LogVerbose(LogGroups.Console, msg);
}
}
/// <summary>
/// Rotate the the drive (positive degrees turn counterclockwise)
/// </summary>
/// <param name="degrees">(positive degrees turn counterclockwise)</param>
/// <param name="power">(-1.0 to 1.0)</param>
IEnumerator<ITask> RotateUntilDegrees(double degrees, double power)
{
#if TRACELOG
Tracer.Trace("^^^^^^^^^^^^^^^^^^^^^ TrackRoamerDriveService:: RotateUntilDegrees(degrees=" + degrees + ", power=" + power + ")");
#endif
EncoderTicksEnabled = false;
//reset encoders
encoder.Reset Lreset = new encoder.Reset();
_leftEncoderPort.Post(Lreset);
yield return (Arbiter.Choice(Lreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
encoder.Reset Rreset = new encoder.Reset();
_rightEncoderPort.Post(Rreset);
yield return (Arbiter.Choice(Rreset.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { LogError(fault); }
));
double arcDistance = Math.Abs(degrees) * _state.DistanceBetweenWheels * Math.PI / 360.0d;
//compute tick to stop at
stopLeftWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.LeftWheel.Radius / _state.LeftWheel.EncoderState.TicksPerRevolution));
stopRightWheelAt = (int)Math.Round(arcDistance / (2.0d * Math.PI * _state.RightWheel.Radius / _state.RightWheel.EncoderState.TicksPerRevolution));
EncoderTicksEnabled = true;
pollEncoderState(); // get starting encoder state
//start moving
// start rotate operation
_state.RotateDegreesStage = drive.DriveStage.Started;
drive.RotateDegrees rotateUpdate = new drive.RotateDegrees();
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Started;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Started");
#endif
_internalDriveOperationsPort.Post(rotateUpdate);
PortSet<DefaultUpdateResponseType, Fault> responsePort = new PortSet<DefaultUpdateResponseType, Fault>();
double rightPow;
double leftPow;
if (degrees > 0)
{
rightPow = power;
leftPow = -power;
}
else
{
rightPow = -power;
leftPow = power;
}
motor.SetMotorPower leftPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = leftPow });
leftPower.ResponsePort = responsePort;
_leftMotorPort.Post(leftPower);
motor.SetMotorPower rightPower = new motor.SetMotorPower(new motor.SetMotorPowerRequest() { TargetPower = rightPow });
rightPower.ResponsePort = responsePort;
_rightMotorPort.Post(rightPower);
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() start moving: degrees=" + degrees);
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() will stop wheels at: Left=" + stopLeftWheelAt + " Right=" + stopRightWheelAt);
#endif
Activate(Arbiter.MultipleItemReceive<DefaultUpdateResponseType, Fault>(responsePort, 2,
delegate(ICollection<DefaultUpdateResponseType> successList, ICollection<Fault> failureList)
{
foreach (Fault fault in failureList)
{
LogError(fault);
}
}
));
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() calling DriveWaitForCompletionDual() - waiting for both sides to complete...");
#endif
yield return DriveWaitForCompletionDual();
#if TRACELOG
Tracer.Trace("=============== TrackRoamerDriveService:: RotateUntilDegrees() - both sides completed, send notification of RotateDegrees complete to subscription manager");
#endif
// send notification of RotateDegrees complete to subscription manager
rotateUpdate.Body.RotateDegreesStage = drive.DriveStage.Completed;
#if TRACELOG
Tracer.Trace("++++++++++++++++++ DRIVE: RotateUntilDegrees() DriveStage.Completed");
#endif
_internalDriveOperationsPort.Post(rotateUpdate);
_state.RotateDegreesStage = drive.DriveStage.Completed;
}
public void PortSetReceiveToBeUsedWithIterators()
{
iterPort = new PortSet (typeof (string), typeof (char), typeof (int));
iterEvent = new AutoResetEvent (false);
iterRes = 0;
using (Dispatcher d = new Dispatcher ()) {
var disp = new DispatcherQueue ("bla", d);
disp.Enqueue (new IterativeTask (this.SimpleTaskIterator));
for (int i = 0; i < 5; ++i)
iterPort.PostUnknownType ((i + 1) * 10);
Assert.IsTrue (iterEvent.WaitOne (2000), "#1");
Assert.AreEqual (150, iterRes, "#2");
}
}
protected override IEnumerator<ITask> SetCallback(vector.SetRequestInfo request, PortSet<vector.CallbackResponseType, Fault> responsePort)
{
if (request is vector.SetAllRequestInfo)
Activate(Arbiter.Choice(
_scribblerPort.SetAllLEDs(new brick.SetAllLedsBody() { LeftLED = _state.GetBool(0), CenterLED = _state.GetBool(1), RightLED = _state.GetBool(2) }),
delegate(DefaultUpdateResponseType success) { responsePort.Post(vector.CallbackResponseType.Instance); },
delegate(Fault failure) { responsePort.Post(failure); }));
else if (request is vector.SetElementsRequestInfo)
{
vector.SetElementsRequestInfo info = (vector.SetElementsRequestInfo)request;
int setLEDScribblerIndex = -1; // will be set to -2 for set all
var responses = new PortSet<DefaultUpdateResponseType, Fault>();
int nResponses = 0;
foreach (int i in info.Indices)
{
if (i >= 0)
if (i <= 2)
if (setLEDScribblerIndex >= 0)
setLEDScribblerIndex = -2;
else
setLEDScribblerIndex = i;
else if (i == 3)
{
nResponses++;
Activate(Arbiter.Choice(_scribblerPort.SetLEDFront(_state.GetBool(3)),
delegate(DefaultUpdateResponseType s) { responses.Post(DefaultUpdateResponseType.Instance); },
delegate(Fault f) { responses.Post(f); }));
}
else if (i == 4)
{
nResponses++;
byte val =
(_state.Get(4) <= 0.0) ?
(byte)0 :
(byte)(_state.Get(4) * (255.0 - 170.0) + 170.0);
Activate(Arbiter.Choice(
_scribblerPort.SetLEDBack(val),
delegate(DefaultUpdateResponseType s) { responses.Post(DefaultUpdateResponseType.Instance); },
delegate(Fault f) { responses.Post(f); }));
}
}
if (setLEDScribblerIndex == -2)
{
nResponses++;
Activate(Arbiter.Choice(
_scribblerPort.SetAllLEDs(new brick.SetAllLedsBody() { LeftLED = _state.GetBool(0), CenterLED = _state.GetBool(1), RightLED = _state.GetBool(2) }),
delegate(DefaultUpdateResponseType s) { responses.Post(DefaultUpdateResponseType.Instance); },
delegate(Fault f) { responses.Post(f); }));
}
else if (setLEDScribblerIndex >= 0)
{
nResponses++;
Activate(Arbiter.Choice(
_scribblerPort.SetLED(new brick.SetLedBody() { LED = setLEDScribblerIndex, State = _state.GetBool(setLEDScribblerIndex) }),
delegate(DefaultUpdateResponseType s) { responses.Post(DefaultUpdateResponseType.Instance); },
delegate(Fault f) { responses.Post(f); }));
}
Activate(Arbiter.MultipleItemReceive(responses, nResponses,
delegate(ICollection<DefaultUpdateResponseType> ss, ICollection<Fault> fs)
{
if (fs.Count == 0)
responsePort.Post(vector.CallbackResponseType.Instance);
else
{
responsePort.Post(fs.First());
////f.Readon could be null
//var reasons = new List<ReasonText>();
//foreach (var f in fs)
// if (f.Reason != null)
// reasons.AddRange(f.Reason.AsEnumerable());
//responsePort.Post(new Fault() { Detail = new Detail() { Any = fs.ToArray() }, Reason = reasons.ToArray() });
}
}));
}
yield break;
}
