public virtual IEnumerator<ITask> GetHandler(Get get) { var write = new LegoLSWrite { Port = _state.SensorPort, TXData = new byte[] {0x10, 0x07}, ExpectedI2CResponseSize = 15 }; Activate(Arbiter.Choice(_legoBrickPort.SendNxtCommand(write), EmptyHandler, f => LogInfo(f.ToException().InnerException + " " + f.ToException().Message))); Activate(Arbiter.Receive(false, TimeoutPort(500), EmptyHandler)); var read = new LegoLSRead(_state.SensorPort); Activate(Arbiter.Choice(_legoBrickPort.SendNxtCommand(read), r => { var response = LegoResponse.Upcast<I2CResponseSensorType>(r); if (response != null) { LogInfo(string.Format("Read response: {0} {1}", response.Manufacturer, response.SensorType)); _state.ManufactureInfo = string.Format("{0}", response.SensorType); //_state.ManufactureInfo = string.Format("Hiya!"); LogInfo(string.Format(_state.ManufactureInfo)); get.ResponsePort.Post(_state); } }, f => { LogInfo(f.Detail.ToString()); get.ResponsePort.Post(f); })); //get.ResponsePort.Post(_state); yield break; }
public virtual IEnumerator<ITask> ReadFromI2cAddressHandler(ReadFromI2cAddress readRequest) { var write = new LegoLSWrite { Port = _state.SensorPort, TXData = readRequest.Body.TxData, ExpectedI2CResponseSize = readRequest.Body.ExpectedResponseSize }; Activate(Arbiter.Choice(_legoBrickPort.SendNxtCommand(write), EmptyHandler, f => LogInfo(f.ToException().InnerException + " " + f.ToException().Message))); Activate(Arbiter.Receive(false, TimeoutPort(80), EmptyHandler)); var read = new LegoLSRead(_state.SensorPort); Activate(Arbiter.Choice(_legoBrickPort.SendNxtCommand(read), r => readRequest.ResponsePort.Post(new ReadResponse {Bytes = r.CommandData}), f => readRequest.ResponsePort.Post(f))); yield break; }
/// <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; }