protected override void Start()
{
LogVerbose(LogGroups.Console, "Configuring Lego Contact Sensor Array... ");
#region Initialize Service State
bool saveState = false;
if (_bumperConfigState == null)
{
_bumperConfigState = new LegoNxtBumperState();
_bumperConfigState.ContactSensorArrayState = new bumper.ContactSensorArrayState();
saveState = true;
}
else if (_bumperConfigState.Status != LegoNxtBumperStatus.Uninitialized)
{
_bumperConfigState.Status = LegoNxtBumperStatus.Uninitialized;
saveState = true;
}
if (saveState)
{
SaveState(_bumperConfigState);
saveState = false;
}
#endregion
// Listen for each of my handlers
Interleave mainInterleave = ActivateDsspOperationHandlers();
// Add the internal only handlers
mainInterleave.CombineWith(
new Interleave(
new ExclusiveReceiverGroup(
Arbiter.Receive <UpdateStatus>(true, _internalPort, UpdateStatusHandler)
),
new ConcurrentReceiverGroup()));
// Publish the service to the local Node Directory
DirectoryInsert();
#region Wait for Configuration to be completed
Port <bool> donePort = new Port <bool>();
SpawnIterator <Port <bool> >(donePort, InitializeBumperConnection);
Activate(Arbiter.Receive(false, donePort,
delegate(bool success)
{
if (success)
{
// display HTTP service Uri
LogInfo(LogGroups.Console, "Service uri: ");
}
else
{
LogError(LogGroups.Console, "LEGO Bumper service failed to start.");
_mainPort.Post(new DsspDefaultDrop());
}
}));
#endregion
}
/// <summary>
/// Multi-stage bumper initialization code
/// </summary>
/// <returns></returns>
private IEnumerator <ITask> InitializeBumperConnection(Port <bool> donePort)
{
bool serviceSuccess = false;
//configure default state
Port <LegoNxtBumperState> configBumperPort = new Port <LegoNxtBumperState>();
SpawnIterator <Port <LegoNxtBumperState> >(configBumperPort, GetLegoBumperConfiguration);
yield return(Arbiter.Receive <LegoNxtBumperState>(false, configBumperPort,
delegate(LegoNxtBumperState newBumperState)
{
_bumperConfigState = newBumperState;
}));
// Subscribe to NXT for touch sensor notifications
if (ValidState(_bumperConfigState.ContactSensorArrayState))
{
SaveState(_bumperConfigState);
Port <bool> subscribeDonePort = new Port <bool>();
SpawnIterator <Port <bool> >(subscribeDonePort, SubscribeToNXT);
yield return(Arbiter.Receive <bool>(false, subscribeDonePort,
delegate(bool success)
{
serviceSuccess = success;
}));
}
if (serviceSuccess)
{
yield return(Arbiter.Choice(
UpdateServiceStatus(LegoNxtBumperStatus.ConnectedToNXT),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { serviceSuccess = false; }));
}
donePort.Post(serviceSuccess);
yield break;
}
/// <summary>
/// Get state from LEGO and set up a contact sensor
/// for each of the configured ports.
/// </summary>
/// <param name="resultPort"></param>
/// <returns></returns>
private IEnumerator <ITask> GetLegoBumperConfiguration(Port <LegoNxtBumperState> resultPort)
{
LegoNxtBumperState configArray = new LegoNxtBumperState();
nxt.LegoNxtState legoState = null;
bool existingBumperConfig = (_bumperConfigState != null && _bumperConfigState.BumperConfigList != null && _bumperConfigState.BumperConfigList.Count > 0);
bool validLegoState = false;
int tryCount = 0;
while (!validLegoState)
{
tryCount++;
// See if the LEGO is configured.
yield return(Arbiter.Choice(_legoPort.Get(),
delegate(nxt.LegoNxtState legoNxtState)
{
legoState = legoNxtState;
},
delegate(Fault fault)
{
LogError(fault);
}));
validLegoState = (legoState != null &&
legoState.BrickConfig != null &&
legoState.BrickConfig.SensorPort != null &&
legoState.BrickConfig.SensorPort.Length > 0 &&
legoState.ComPort > 0);
// If we don't have valid configuration from the NXT Brick
// wait a little and try again.
if (!validLegoState)
{
LogVerbose(LogGroups.Console, "Waiting for LEGO NXT to be initialized");
System.Threading.Thread.Sleep(500);
}
}
configArray.ContactSensorArrayState = new bumper.ContactSensorArrayState();
configArray.ContactSensorArrayState.Sensors = new List <bumper.ContactSensor>();
configArray.BumperConfigList = new List <LegoNxtBumperConfig>();
// We do not have a valid configuration from the NXT.
if (!validLegoState)
{
// Do we have a valid prior configuration?
if (existingBumperConfig)
{
configArray.BumperConfigList = _bumperConfigState.BumperConfigList;
configArray.ContactSensorArrayState = _bumperConfigState.ContactSensorArrayState;
}
else
{
// We have no prior configuration
LegoNxtBumperConfig bumperConfig = new LegoNxtBumperConfig();
bumperConfig.HardwareIdentifier = 1;
bumperConfig.SensorType = nxt.SensorDefinition.SensorType.Touch;
bumperConfig.Name = "Default Bumper 1";
bumperConfig.ThresholdLow = 1;
bumperConfig.ThresholdHigh = 1;
configArray.BumperConfigList.Add(bumperConfig);
bumper.ContactSensor defaultBumper = new bumper.ContactSensor();
defaultBumper.HardwareIdentifier = bumperConfig.HardwareIdentifier;
defaultBumper.Name = bumperConfig.Name;
configArray.ContactSensorArrayState.Sensors.Add(defaultBumper);
}
}
else // We have valid LEGO NXT Configuration
{
int hardwareIdentifier = 1;
foreach (nxt.SensorConfig sensorConfig in legoState.BrickConfig.SensorPort)
{
LegoNxtBumperConfig bumperConfig = new LegoNxtBumperConfig();
bumperConfig.HardwareIdentifier = hardwareIdentifier;
bumperConfig.SensorType = sensorConfig.Type;
if (sensorConfig.Type == nxt.SensorDefinition.SensorType.Touch)
{
bumperConfig.Name = "Touch Bumper " + hardwareIdentifier.ToString();
bumperConfig.ThresholdLow = 1;
bumperConfig.ThresholdHigh = 1;
}
else if (sensorConfig.Type == nxt.SensorDefinition.SensorType.Sonar)
{
bumperConfig.Name = "Sonar Bumper " + hardwareIdentifier.ToString();
if (sensorConfig.ExternalRange)
{
bumperConfig.ThresholdLow = sensorConfig.LowThresh;
bumperConfig.ThresholdHigh = sensorConfig.HighThresh;
}
else
{
bumperConfig.ThresholdLow = 0;
bumperConfig.ThresholdHigh = 15;
}
}
// Sound or Light with an external range for a trigger?
else if (sensorConfig.ExternalRange &&
(0 != (sensorConfig.Type &
(nxt.SensorDefinition.SensorType.LightOn
| nxt.SensorDefinition.SensorType.LightOff
| nxt.SensorDefinition.SensorType.Sound))))
{
bumperConfig.Name = sensorConfig.Type.ToString() + " Sensor " + hardwareIdentifier.ToString();
bumperConfig.ThresholdLow = sensorConfig.LowThresh;
bumperConfig.ThresholdHigh = sensorConfig.HighThresh;
}
#region Merge with prior config
// Look to see if this sensor was already configured
if (existingBumperConfig)
{
LegoNxtBumperConfig prevBumperConfig = _bumperConfigState.BumperConfigList.Find(delegate(LegoNxtBumperConfig cfg) { return(cfg.HardwareIdentifier == hardwareIdentifier); });
if (prevBumperConfig != null)
{
// yes, use the old name.
bumperConfig.Name = prevBumperConfig.Name;
// if the sensor type has not changed, use the old threshold values.
if (prevBumperConfig.SensorType == bumperConfig.SensorType)
{
bumperConfig.ThresholdLow = prevBumperConfig.ThresholdLow;
bumperConfig.ThresholdHigh = prevBumperConfig.ThresholdHigh;
}
}
}
#endregion
if (!string.IsNullOrEmpty(bumperConfig.Name))
{
configArray.BumperConfigList.Add(bumperConfig);
configArray.ContactSensorArrayState.Sensors.Add(new bumper.ContactSensor(hardwareIdentifier, bumperConfig.Name));
}
hardwareIdentifier++;
}
}
resultPort.Post(configArray);
yield break;
}
