public virtual IEnumerator<ITask> ConnectToHardwareHandler(ConnectToHardware configure)
{
bool success = true;
Fault fault = null;
try
{
if (_state.Runtime.Connected)
{
// If already connected, ConnectToHardware always forces a connection reset.
_internalStatus.ClosePending = true;
_state.Runtime.Connected = false;
SendNotification<DisconnectFromHardware>(_subMgrPort, new DisconnectFromHardware());
}
_state.Configuration = configure.Body;
if (configure.Body.SerialPort <= 0)
{
// Close the Connection
if (_state.Runtime.Connected)
{
_internalStatus.Disconnected = true;
yield return Arbiter.Choice(_brickPort.Close(),
EmptyHandler<DefaultSubmitResponseType>,
delegate(Fault failure)
{
success = false;
fault = failure;
});
}
}
else
{
// Open the Connection
yield return Arbiter.Choice(
_brickPort.Open( new comm.OpenRequest(configure.Body.SerialPort, configure.Body.BaudRate, configure.Body.ConnectionType)),
delegate(DefaultSubmitResponseType ok)
{
_internalStatus.Disconnected = false;
_state.Runtime.Connected = true;
},
delegate(Fault failure)
{
fault = failure;
_state.Runtime.Connected = false;
success = false;
});
if (_state.Runtime.Connected)
{
LegoGetFirmwareVersion getFirmwareVersion = new LegoGetFirmwareVersion();
getFirmwareVersion.TryCount = 3;
yield return Arbiter.Choice(_brickPort.SendCommand(getFirmwareVersion),
delegate(LegoResponse ok)
{
LegoResponseGetFirmwareVersion fv = LegoResponse.Upcast<LegoResponseGetFirmwareVersion>(ok);
if (fv != null && fv.Success)
{
_state.Runtime.Firmware = string.Format("Firmware: {0}.{1} Protocol: {2}.{3}", fv.MajorFirmwareVersion, fv.MinorFirmwareVersion, fv.MajorProtocolVersion, fv.MinorProtocolVersion);
if (fv.MajorFirmwareVersion == 1 && fv.MinorFirmwareVersion < 3)
LogError(LogGroups.Console, "Your LEGO Firmware is out of date. \n"
+ "Please refer to the LEGO MINDSTORMS website \n"
+ "and use the LEGO MINDSTORMS Software to update your firmware.");
}
else
{
LogError(LogGroups.Console, fv.ErrorCode.ToString());
success = false;
}
},
delegate(Fault failure)
{
success = false;
_state.Runtime.Connected = false;
fault = failure;
});
if (_state.Runtime.Connected)
yield return Arbiter.Choice(_brickPort.SendCommand(new LegoGetDeviceInfo()),
delegate(LegoResponse ok)
{
LegoResponseGetDeviceInfo fv = LegoResponse.Upcast<LegoResponseGetDeviceInfo>(ok);
if (fv != null
&& fv.Success
&& !string.IsNullOrEmpty(fv.BrickName))
{
_state.Runtime.BrickName = fv.BrickName;
}
},
delegate(Fault failure)
{
_state.Runtime.Connected = false;
success = false;
fault = failure;
});
// If we already had registered devices
// we need to reinitialize them!
if (success
&& _state.Runtime.Connected
&& _state.Runtime.Devices != null
&& _state.Runtime.Devices.Count > 0)
{
foreach (AttachRequest device in _state.Runtime.Devices.Values)
{
if (device.InitializationCommands != null)
{
foreach (LegoCommand cmd in device.InitializationCommands.Commands)
{
// Send Initialization Sequence.
// If an LSWrite is sent here, it will only send the command
// without LSReading a response.
yield return Arbiter.Choice(_brickPort.SendCommand(cmd),
delegate(LegoResponse ok) { },
delegate(Fault failure) { LogError(failure); });
}
}
}
}
else if (!_state.Runtime.Connected)
{
if (configure.Body.SerialPort != 0)
success = false;
// We failed to talk to the brick, even though the port is open.
// Clean up by closing the serial port.
yield return Arbiter.Choice(_brickPort.Close(),
EmptyHandler<DefaultSubmitResponseType>,
EmptyHandler<Fault>);
}
}
// Send a notification that the Brick service is ready.
if (success && _state.Runtime.Connected)
{
// Play a tone to signal that initialization was successful.
yield return Arbiter.Choice(_brickPort.SendCommand(new LegoPlayTone(1074, 500)),
delegate(LegoResponse ok) { },
delegate(Fault failure) { LogError(failure); });
SendNotification<ConnectToHardware>(_subMgrPort, configure);
}
}
}
finally
{
if (success)
{
configure.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
else
{
if (fault == null)
fault = Fault.FromException(new InvalidOperationException("Failed to connect to the LEGO NXT Hardware"));
LogError(fault);
configure.ResponsePort.Post(fault);
}
}
yield break;
}
/// <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;
}
