/// <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); }