/// <summary>
/// Handle sensor update message from NXT
/// </summary>
private void SensorNotificationHandler(nxt.Configure notify)
{
//update state
foreach (bumper.ContactSensor sensor in _bumperConfigState.ContactSensorArrayState.Sensors)
{
bool newContactValue;
LegoNxtBumperConfig bumperConfig = GetBumperConfig(sensor.HardwareIdentifier);
if (bumperConfig == null)
{
// Old logic works only for touch sensor
newContactValue = (notify.Body.SensorPort[sensor.HardwareIdentifier - 1] == 1) ? true : false;
}
else
{
newContactValue = ((notify.Body.SensorPort[sensor.HardwareIdentifier - 1] >= bumperConfig.ThresholdLow) &&
(notify.Body.SensorPort[sensor.HardwareIdentifier - 1] <= bumperConfig.ThresholdHigh));
}
bool changed = (sensor.Pressed != newContactValue);
sensor.TimeStamp = DateTime.Now;
sensor.Pressed = newContactValue;
if (changed)
{
//notify subscribers on any bumper pressed or unpressed
SendNotification <bumper.Update>(_subMgrPort, sensor);
}
}
}
/// <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;
}
