/// <summary>
/// Get the integer representation of a LegoNxtPort (zero based).
/// </summary>
/// <param name="port"></param>
/// <returns></returns>
public static byte PortNumber(LegoNxtPort port)
{
switch (port)
{
case LegoNxtPort.A:
case LegoNxtPort.EncoderA:
case LegoNxtPort.MotorA:
case LegoNxtPort.Sensor1:
return 0;
case LegoNxtPort.B:
case LegoNxtPort.EncoderB:
case LegoNxtPort.MotorB:
case LegoNxtPort.Sensor2:
return 1;
case LegoNxtPort.C:
case LegoNxtPort.EncoderC:
case LegoNxtPort.MotorC:
case LegoNxtPort.Sensor3:
return 2;
case LegoNxtPort.Sensor4:
return 3;
default:
return 0;
}
}
/// <summary>
/// Get the HardwareIdentifier representation of a LegoNxtPort (1-4).
/// </summary>
/// <param name="port"></param>
/// <returns></returns>
public static int HardwareIdentifier(LegoNxtPort port)
{
return ((int)(PortNumber(port) + 1));
}
/// <summary>
/// Test a Sensor Port for an I2C Sensor
/// </summary>
/// <param name="response"></param>
/// <param name="sensorPort"></param>
/// <param name="sensorType"></param>
/// <returns></returns>
private IEnumerator<ITask> TestPortForI2CSensorHandler(Port<bool> response, LegoNxtPort sensorPort, string sensorType)
{
LogInfo("- TestPortForI2CSensorHandler");
//Debugger.Break();
// Read from I2C to find the device.
bool found = false;
bool abort = false;
LegoSetInputMode setInputMode = null;
if (_brickPort != null)
{
// Configure the port as an I2C sensor.
setInputMode = new LegoSetInputMode((NxtSensorPort)sensorPort, LegoSensorType.I2C_9V, LegoSensorMode.RawMode);
setInputMode.TryCount = 2;
yield return Arbiter.Choice(_brickPort.SendCommand(setInputMode),
delegate(LegoResponse ok)
{
//Debugger.Break();
if (!ok.Success)
abort = true;
else
LogInfo(LogGroups.Console, string.Format("{0} set to {1} mode.", sensorPort, setInputMode.SensorType));
},
delegate(Fault fault)
{
abort = true;
});
}
else
abort = true;
if (abort)
{
LogInfo(LogGroups.Console, string.Format("Failure setting I2C mode on {0}.", sensorPort));
response.Post(false);
yield break;
}
SendLowSpeedCommand lsCmd = new SendLowSpeedCommand();
I2CReadSensorType readSensors = new I2CReadSensorType((NxtSensorPort)sensorPort);
LogInfo("The sensor port is: " + sensorPort.ToString());
lsCmd.Body = readSensors;
SpawnIterator<SendLowSpeedCommand>(lsCmd, SendLowSpeedCommandHandler);
yield return Arbiter.Choice(lsCmd.ResponsePort,
delegate(LegoResponse ok)
{
//Debugger.Break();
I2CResponseSensorType sensorResponse = LegoResponse.Upcast<I2CResponseSensorType>(ok);
LogInfo(LogGroups.Console, string.Format("{0} I2C response {1} is {2}.", sensorPort, sensorResponse.ErrorCode, sensorResponse.SensorType));
if (sensorResponse.SensorType.Equals(sensorType, StringComparison.InvariantCultureIgnoreCase))
{
found = true;
}
else if (sensorType.IndexOf(',') >= 0)
{
foreach (string subtype in sensorType.Split(','))
{
if (sensorResponse.SensorType.Equals(subtype, StringComparison.InvariantCultureIgnoreCase))
found = true;
}
}
else
{
LogInfo(LogGroups.Console, string.Format("Found an unattached I2C Sensor from {0}: {1}", sensorResponse.Manufacturer, sensorResponse.SensorType));
}
},
delegate(Fault fault)
{
string msg = (fault.Reason != null && fault.Reason.Length > 0 && !string.IsNullOrEmpty(fault.Reason[0].Value)) ? fault.Reason[0].Value : string.Empty;
LogError(LogGroups.Console, string.Format("{0} fault reading I2C Sensor Type: {1}.", sensorPort, msg));
abort = true;
});
if (!found)
{
LogInfo(LogGroups.Console, string.Format("Set {0} back to No Sensor.", sensorPort));
setInputMode = new LegoSetInputMode((NxtSensorPort)sensorPort, LegoSensorType.NoSensor, LegoSensorMode.RawMode);
yield return Arbiter.Choice(_brickPort.SendCommand(setInputMode),
delegate(LegoResponse ok)
{
if (!ok.Success)
abort = true;
},
delegate(Fault fault)
{
abort = true;
});
}
LogInfo(LogGroups.Console, string.Format("I2C ReadSensorType on {0} finished: {1}", sensorPort, found));
response.Post(found);
yield break;
}
/// <summary>
/// A connection to the LEGO NXT Brick
/// </summary>
public LegoNxtConnection(LegoNxtPort port)
{
this._port = port;
}
/// <summary>
/// Test a Sensor Port for an I2C Sensor
/// </summary>
/// <param name="sensorPort"></param>
/// <param name="sensorType"></param>
/// <returns></returns>
private Port<bool> TestPortForI2CSensor(LegoNxtPort sensorPort, string sensorType)
{
Port<bool> resultPort = new Port<bool>();
if (_state.Runtime.Devices.ContainsKey(sensorPort.ToString()))
{
LogInfo(LogGroups.Console, string.Format("I2C Sensor Port {0} is already reserved.", sensorPort));
// If the port is reserved, don't even look at it.
resultPort.Post(false);
}
else
{
SpawnIterator<Port<bool>, LegoNxtPort, string>(resultPort, sensorPort, sensorType, TestPortForI2CSensorHandler);
}
return resultPort;
}
