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