public int ReadEncoder(Motor motor)
{
var buffer = new[] { (byte)Commands.ReadEncoder, (byte)motor, Constants.Unused, Constants.Unused };
_goPiGo.WriteToI2C(buffer);
try
{
var b1 = _goPiGo.ReadByte();
Task.Delay(5).Wait();
var b2 = _goPiGo.ReadByte();
return(b1 * 256 + b2);
}
catch (Exception)
{
return(-1);
}
}