private void ProcessMessage(mBlockIncomingMessage message)
{
lock (_messageHandlers)
{
var associatedIncomingHandler = _messageHandlers.Where(msg => msg.MessageSerialNumber == message.MessageSerialNumber).FirstOrDefault();
if (associatedIncomingHandler != null)
{
associatedIncomingHandler.Handler(message);
_messageHandlers.Remove(associatedIncomingHandler);
}
}
}
public void ProcessSonar(mBlockIncomingMessage message)
{
FrontIRSensor = Convert.ToInt32(_currentIncomingMessage.FloatPayload);
var factor = Speed / 100;
if (FrontIRSensor < (10 * factor) && CurrentState == Commands.Forward)
{
PauseRefreshTimer();
SendCommand(Commands.Stop);
StartRefreshTimer();
}
}
public void ProcessSonar(mBlockIncomingMessage message)
{
FrontSonar = Convert.ToInt32(_currentIncomingMessage.FloatPayload);
LastBotContact = DateTime.Now;
var factor = Speed / 100;
if (FrontSonar < (10 * factor) && CurrentState == Commands.Forward)
{
_logger.NotifyUserError("mTankBot_ProcessSonar", "Sending Stop Command");
PauseRefreshTimer();
SendCommand(Commands.Stop);
StartRefreshTimer();
}
}
private void ProcessBuffer(byte[] buffer)
{
if (_currentIncomingMessage == null)
{
_currentIncomingMessage = new mBlockIncomingMessage();
}
foreach (var value in buffer)
{
/* Received message format
* 0xFF - Header Byte 1
* 0x55 - Header Byte 2
* 0xXX - Message index corresponding to request
* 0x0X - Payload Type - 1 byte 2 float 3 short 4 len+string 5 double
* [0xXX....0xXX] Payload matcing size
* 0x0D
* 0x0A
*/
_currentIncomingMessage.AddByte(value);
if (_currentIncomingMessage.EndsWithCRLF())
{
IncomingMessages.Add(_currentIncomingMessage);
if (_currentIncomingMessage.BufferSize > 4)
{
_currentIncomingMessage.MessageSerialNumber = _currentIncomingMessage.Buffer[2];
_logger.NotifyUserInfo("mBlock", $"<<< {_currentIncomingMessage.MessageSerialNumber:X2}. " + _currentIncomingMessage.MessageHexString);
ProcessMessage(_currentIncomingMessage);
}
else
{
_logger.NotifyUserInfo("mBlock", $"<<< XXXXX. " + _currentIncomingMessage.MessageHexString);
}
_currentIncomingMessage = new mBlockIncomingMessage();
}
}
}
public void ProcessVersion(mBlockIncomingMessage message)
{
if (!String.IsNullOrEmpty(message.StringPayload))
{
var parts = message.StringPayload.Split('.');
int majorVersion;
Int32.TryParse(parts[0], out majorVersion);
if (majorVersion == 5)
{
DeviceMode = DeviceModes.CustomFirmware;
APIMode = "Custom mBot";
}
else if (majorVersion == 9)
{
DeviceMode = DeviceModes.CustomFirmwareRanger;
APIMode = "Ranger Firmware";
}
else
{
APIMode = "Stock mBot";
}
FirmwareVersion = message.StringPayload;
if (!_connectedToBot)
{
_logger.NotifyUserError("mTankBot_RequestVersion", "Made Contact with mBot");
PlayTone(294);
SetLED(0, NamedColors.Yellow);
}
LastBotContact = DateTime.Now;
StartSensorRefreshTimer();
_connectedToBot = true;
}
}
public void ProcessVersion(mBlockIncomingMessage message)
{
if (!String.IsNullOrEmpty(message.StringPayload))
{
var parts = message.StringPayload.Split('.');
int majorVersion;
Int32.TryParse(parts[0], out majorVersion);
if (majorVersion == 5)
{
DeviceMode = DeviceModes.CustomFirmware;
APIMode = "Custom mBot";
}
else if (majorVersion == 9)
{
DeviceMode = DeviceModes.CustomFirmwareRanger;
APIMode = "Ranger Firmware";
}
else
{
APIMode = "Stock mBot";
}
FirmwareVersion = message.StringPayload;
if (!_connectedToBot)
{
PlayTone(294);
SetLED(0, NamedColors.Yellow);
}
this.LastBotContact = DateTime.Now;
_connectedToBot = true;
}
}
private void ProcessBuffer(byte[] buffer)
{
if (_currentIncomingMessage == null)
{
_currentIncomingMessage = new mBlockIncomingMessage();
}
foreach (var value in buffer)
{
/* Received message format
* 0xFF - Header Byte 1
* 0x55 - Header Byte 2
* 0xXX - Message index corresponding to request
* 0x0X - Payload Type - 1 byte 2 float 3 short 4 len+string 5 double
* [0xXX....0xXX] Payload matcing size
* 0x0D
* 0x0A
*/
_currentIncomingMessage.AddByte(value);
if (_currentIncomingMessage.EndsWithCRLF())
{
IncomingMessages.Add(_currentIncomingMessage);
// Debug.WriteLine(String.Format("{0:000000} <<<", (DateTime.Now - _start).TotalMilliseconds) + _currentIncomingMessage.MessageHexString);
_logger.NotifyUserInfo("mBlock", "<<< " + _currentIncomingMessage.MessageHexString);
if (_currentIncomingMessage.BufferSize > 4)
{
_currentIncomingMessage.MessageSerialNumber = _currentIncomingMessage.Buffer[2];
ProcessMessage(_currentIncomingMessage);
}
_currentIncomingMessage = new mBlockIncomingMessage();
}
}
}
