/// <summary>
/// Send I2C Command and post a response when it is completed.
/// </summary>
/// <param name="cmd"></param>
/// <returns></returns>
public virtual IEnumerator<ITask> I2cLowSpeedCommandHandler(SendLowSpeedCommand cmd)
{
bool done = false;
bool retry = false;
cmd.Body.RequireResponse = false;
cmd.Body.TryCount = 1;
yield return Arbiter.Choice(_brickPort.SendCommand(cmd.Body),
delegate(LegoResponse rsp)
{
if (cmd.Body.ExpectedI2CResponseSize == 0)
{
cmd.ResponsePort.Post(new LegoResponseLSRead());
done = true;
}
},
delegate(Fault fault)
{
cmd.ResponsePort.Post(fault);
done = true;
});
if (done)
yield break;
// Call LSStatus until the data is ready
_cmdLSGetStatus.Port = cmd.Body.Port;
_cmdLSGetStatus.TryCount = 1;
int lsBytesReady = 0;
int statusReadCount = 0;
do
{
//Debugger.Break();
yield return Arbiter.Choice(_brickPort.SendCommand(_cmdLSGetStatus),
delegate(LegoResponse rsp)
{
LegoResponseLSGetStatus lsStatus = rsp as LegoResponseLSGetStatus;
if (lsStatus == null)
{
////Debugger.Break();
lsStatus = new LegoResponseLSGetStatus(rsp.CommandData);
}
switch (lsStatus.ErrorCode)
{
case LegoErrorCode.Success:
lsBytesReady = lsStatus.BytesReady;
break;
case LegoErrorCode.PendingCommunicationTransactionInProgress:
// Just try status again
break;
case LegoErrorCode.CommunicationBusError:
retry = true;
break;
default:
cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Lego Error Code: " + lsStatus.ErrorCode)));
done = true;
break;
}
},
delegate(Fault fault)
{
cmd.ResponsePort.Post(fault);
done = true;
});
if (retry)
{
retry = false;
// Reset the I2C port by reading 1 byte.
LegoLSWrite resetI2C = new I2CReadSonarSensor(cmd.Body.Port, UltraSonicPacket.FactoryZero);
//LegoLSWrite resetI2C = new I2CReadSonarSensor(cmd.Body.Port, UltraSonicPacket.ByteEight);
SendLowSpeedCommand sendReset = new SendLowSpeedCommand(resetI2C);
if (cmd.Body.ExpectedI2CResponseSize == 1 && NxtCommon.ByteArrayIsEqual(cmd.Body.CommandData, resetI2C.CommandData))
{
// we received a BusError on the reset command.
cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("Lego Error Code: " + LegoErrorCode.CommunicationBusError)));
done = true;
}
else
{
LogInfo(LogGroups.Console, "Resetting the I2C Bus on " + cmd.Body.Port.ToString());
SpawnIterator<SendLowSpeedCommand>(sendReset, I2cLowSpeedCommandHandler);
yield return Arbiter.Choice(sendReset.ResponsePort,
delegate(LegoResponse ok)
{
// try again
lsBytesReady = 0;
},
delegate(Fault fault)
{
cmd.ResponsePort.Post(fault);
done = true;
});
}
}
if (done)
yield break;
} while (lsBytesReady < cmd.Body.ExpectedI2CResponseSize && (statusReadCount++ < 10));
// Call LSRead to get the return packet
LegoLSRead lsRead = new LegoLSRead(cmd.Body.Port);
LegoResponse response = null;
Fault lsReadFault = null;
//Debugger.Break();
while (lsBytesReady > 0)
{
yield return Arbiter.Choice(
_brickPort.SendCommand(lsRead),
delegate(LegoResponse rsp) { response = rsp; },
delegate(Fault fault) { lsReadFault = fault; });
lsBytesReady -= cmd.Body.ExpectedI2CResponseSize;
}
//Debugger.Break();
//if (response != null && lsBytesReady == 0 && response.CommandData != null && response.CommandData.Length >= 3 && response.CommandData[2] == 0)
if (response != null && response.CommandData != null && response.CommandData.Length >= 3 && response.CommandData[2] == 0)
cmd.ResponsePort.Post(response);
else if (lsReadFault != null)
cmd.ResponsePort.Post(lsReadFault);
else
cmd.ResponsePort.Post(Fault.FromException(new InvalidOperationException("I2C Communication Error")));
yield break;
}
public virtual IEnumerator<ITask> SendLowSpeedCommandHandler(SendLowSpeedCommand cmd)
{
_internalI2CPort.Post(cmd);
yield break;
}
/// <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>
/// Send Any Command, including a two-phase LowSpeed command (LSWrite)
/// </summary>
/// <param name="cmd"></param>
/// <param name="priority"></param>
/// <returns></returns>
public PortSet<LegoResponse, Fault> SendAnyCommand(LegoCommand cmd, bool priority)
{
if (cmd.LegoCommandCode == LegoCommandCode.LSWrite)
{
LegoLSWrite cmdLsWrite = cmd as LegoLSWrite;
if (cmdLsWrite == null)
cmdLsWrite = new LegoLSWrite(cmd);
// Send Low Speed Command
SendLowSpeedCommand sendCmd = new SendLowSpeedCommand(cmdLsWrite);
_internalI2CPort.Post(sendCmd);
return sendCmd.ResponsePort;
}
return _brickPort.SendCommand(cmd, priority);
}
