public override TimeSpan?InitializeSensorRequest(ProtocolArray requestData)
{
TimeSpan?result = base.InitializeSensorRequest(requestData);
//I2C setup
if (SensorType == SensorType.I2C || SensorType == SensorType.I2C_9V || SensorType == SensorType.ULTRASONIC_CONT)
{
requestData.SetBits(3, 8, Speed);
requestData.SetBits(3, 3, (uint)(I2CData.Count > 8 ? 8 : (I2CData.Count < 1 ? 1 : I2CData.Count)) - 1);
foreach (I2CData device in I2CData)
{
requestData.SetBits(3, 7, (uint)(device.Address >> 1));
requestData.SetBits(3, 2, (uint)device.Setting);
if ((device.Setting & Const.BIT_I2C_SAME) == Const.BIT_I2C_SAME)
{
requestData.SetBits(3, 4, (uint)device.WriteBytes);
requestData.SetBits(3, 4, (uint)device.ReadBytes);
for (int out_byte = 0; out_byte < device.WriteBytes; out_byte++)
{
requestData.SetBits(3, 8, (uint)device.WriteData[out_byte]);
}
}
}
}
return(result);
}
public virtual void UpdateResponse(ProtocolArray responseData, byte encoderBits)
{
uint tacho = responseData.GetBits(1, encoderBits);
Encoder = (int)((tacho >> 1) * (1 + (-(tacho & 0x01) << 1))); //odd numbers are negative speed/backward direction
EncoderOffset = 0;
}
public override void UpdateSensorResponse(ProtocolArray responseData)
{
RawValue = (int)responseData.GetBits(1, (byte)I2CData.Count);
int deviceIndex = 0;
foreach (I2CData device in I2CData)
{
if ((RawValue & (0x01 << deviceIndex)) != 0)
{
for (int in_byte = 0; in_byte < device.ReadBytes; in_byte++)
{
device.ReadData[in_byte] = (byte)responseData.GetBits(1, 8);
}
}
deviceIndex++;
}
if (SensorType == SensorType.ULTRASONIC_CONT)
{
if (((int)RawValue & 0x01) != 0)
{
RawValue = I2CData[Const.US_I2C_IDX].ReadData[0];
}
else
{
RawValue = -1;
}
}
}
public void DataArrayPositionAfterGetWithOffSetTest()
{
ProtocolArray array = new ProtocolArray();
array.GetBits(1, 4);
Assert.AreEqual(4, array.Position);
}
public void DataArrayAddGetBits()
{
ProtocolArray array = new ProtocolArray();
array.SetBits(0, 10, 421);
array.SetBits(0, 10, 751);
array.SetBits(0, 10, 637);
array.SetBits(0, 2, 3);
Assert.AreEqual(32, array.Position);
array.Position = 0;
Assert.AreEqual(array.GetBits(0, 4), (uint)5);
Assert.AreEqual(array.GetBits(0, 4), (uint)10);
Assert.AreEqual(array.GetBits(0, 4), (uint)13);
Assert.AreEqual(array.GetBits(0, 4), (uint)11);
Assert.AreEqual(array.GetBits(0, 4), (uint)11);
Assert.AreEqual(array.GetBits(0, 4), (uint)13);
Assert.AreEqual(array.GetBits(0, 4), (uint)7);
Assert.AreEqual(array.GetBits(0, 4), (uint)14);
array.Position = 0;
Assert.AreEqual(array.GetBits(0, 10), (uint)421);
Assert.AreEqual(array.GetBits(0, 10), (uint)751);
Assert.AreEqual(array.GetBits(0, 10), (uint)637);
Assert.AreEqual(array.GetBits(0, 2), (uint)3);
Assert.AreEqual(array[0], 165);
Assert.AreEqual(array[1], 189);
Assert.AreEqual(array[2], 219);
Assert.AreEqual(array[3], 231);
}
public override void UpdateSensorResponse(ProtocolArray responseData)
{
bool state = Pressed;
if (SensorType == SensorType.TOUCH) //Touch has single bit response, vs Touch_Debounce reads a analog raw value
{
RawValue = (int)responseData.GetBits(1, 1);
}
else
{
base.UpdateSensorResponse(responseData);
}
Pressed = (RawValue != 0);
if (state != Pressed)
{
this.OnChangedEventHandler(new TouchSensorEventArgs()
{
Pressed = this.Pressed
});
if (Pressed)
{
if (null != OnPressed)
{
Task.Run(() => OnPressed(this, new SensorEventArgs()));
}
}
else
{
if (null != OnReleased)
{
Task.Run(() => OnReleased(this, new SensorEventArgs()));
}
}
}
}
private async Task <bool> BrickPiInitializeSensors()
{
bool result = true;
foreach (Arduino arduino in EnumExtension <Arduino> .All())
{
ProtocolArray commandData = new ProtocolArray();
TimeSpan? setupTime = null;
int retry = 0;
commandData[Const.MessageTypeIndex] = (byte)MessageType.ChangeSensorType;
foreach (SensorPort sensorPort in arduino.SensorPorts())
{
setupTime = (sensors[sensorPort].InitializeSensorRequest(commandData).Max(setupTime));
}
while (retry++ < 3)
{
int bytes = 3 + commandData.Bytes;
#if DEBUGMESSAGES
Debug.WriteLine($"BrickPiInitializeSensors - {{arduino.ToString()}");
#endif
byte[] resultData = await BrickPiTxAndRx(arduino, bytes, commandData.Data, null, setupTime).ConfigureAwait(false);
if (null != resultData && resultData.Length == 1 && ((MessageType)resultData[Const.MessageTypeIndex] == MessageType.ChangeSensorType))
{
result &= true;
break;
}
Debug.WriteLine($"Trying again to setup sensors on Arduino {arduino.ToString()}");
await Task.Delay(100).ConfigureAwait(false);
}
}
return(result);
}
public void DataArrayPositionAfterAddNoOffsetTest()
{
ProtocolArray array = new ProtocolArray();
array.SetBits(0, 4, 123456);
Assert.AreEqual(4, array.Position);
}
public virtual void UpdateVelocityRequest(ProtocolArray requestData)
{
int speed = Velocity;
speed = ((Math.Abs(speed) & 0xFF) << 1 | -(Math.Sign(speed) >> 1)) << 1 | (Enabled.ToInt() & 0x01);
requestData.SetBits(1, 10, (uint)speed);
}
public void DataArrayAddMultipleBitsCorrectRange2()
{
ProtocolArray array = new ProtocolArray();
array.SetBits(0, 1, 1);
array.SetBits(0, 5, 20);
Assert.AreEqual(41, BitConverter.ToInt32(array.Data, 0));
}
public void DataArrayAddBitsCorrectRange()
{
uint value = 15;
ProtocolArray array = new ProtocolArray();
array.SetBits(0, 4, value);
Assert.AreEqual(value, BitConverter.ToUInt32(array.Data, 0));
}
public void DataArrayAddBitsRangeOverflow()
{
uint value = 16;
ProtocolArray array = new ProtocolArray();
array.SetBits(0, 4, value);
Assert.AreNotEqual(value, BitConverter.ToInt32(array.Data, 0));
}
public void DataArraySetGetSingleBitTest()
{
ProtocolArray array = new ProtocolArray(8);
array.SetBits(0, 1, 1);
Assert.AreEqual(1, BitConverter.ToInt32(array.Data, 0));
array.Position = 0;
Assert.AreEqual(1, (int)array.GetBits(0, 1));
}
/// <summary>
/// Used to change the address of the Arduino
/// </summary>
/// <param name="currentAddress">current address</param>
/// <param name="newAddress">new address</param>
/// <returns></returns>
private async Task <bool> BrickPiChangeAddress(Arduino arduino, byte newAddress)
{
ProtocolArray dataArray = new ProtocolArray();
dataArray[Const.MessageTypeIndex] = (int)MessageType.ChangeAddress;
dataArray[Const.NewAddressIndex] = newAddress;
#if DEBUGMESSAGES
Debug.WriteLine($"BrickPiSetTimeout - {arduino.ToString()}");
#endif
byte[] result = await BrickPiTxAndRx(arduino, 2, dataArray.Data).ConfigureAwait(false);
return(result != null && result.Length > 1 && (MessageType)result[Const.MessageTypeIndex] == MessageType.ChangeAddress);
}
public override TimeSpan?InitializeSensorRequest(ProtocolArray requestData)
{
TimeSpan?result;
result = base.InitializeSensorRequest(requestData);
if (SensorType == SensorType.ULTRASONIC_CONT)
{
//override sensortype to fake an I2C
requestData[Const.SensorIndex + (int)SensorPort.ArduinoPort()] = (byte)SensorType.I2C;
}
return(result);
}
public void DataArrayAddMultipleBitsCorrectRange()
{
//byte offset = 3;
for (byte offset = 1; offset < 16; offset++)
{
UInt64 result = 0;
ProtocolArray array = new ProtocolArray();
for (int i = 0; i < 8; i++)
{
array.SetBits(0, offset, 1);
//array.AddBits(0, 0, offset, 1);
result += ((ulong)1 << (offset * i));
Assert.AreEqual(result, BitConverter.ToUInt64(array.Data, 0), string.Format("Failed at offset {0} in step {1}", offset, i));
}
}
}
public override void UpdateSensorResponse(ProtocolArray responseData)
{
if (SensorType == SensorType.COLOR_FULL)
{
RawValue = (int)responseData.GetBits(1, 3);
colorData = new ARGBColor(
(int)responseData.GetBits(1, 10),
(int)responseData.GetBits(1, 10),
(int)responseData.GetBits(1, 10),
(int)responseData.GetBits(1, 10));
}
else
{
base.UpdateSensorResponse(responseData);
}
}
/// <summary>
/// Set a new motor timeout on the brick
/// </summary>
private async Task <bool> BrickPiSetTimeout(int timeout)
{
ProtocolArray dataArray = new ProtocolArray(5);
bool result = true;
foreach (Arduino arduino in EnumExtension <Arduino> .All())
{
dataArray[Const.MessageTypeIndex] = (byte)MessageType.ChangeTimeout;
Array.Copy(BitConverter.GetBytes(timeout), 0, dataArray.Data, Const.TimeoutIndex, 4);
#if DEBUGMESSAGES
Debug.WriteLine($"BrickPiSetTimeout - {{arduino.ToString()}");
#endif
byte[] resultData = await BrickPiTxAndRx(arduino, 5, dataArray.Data).ConfigureAwait(false);
result &= (null != resultData && resultData.Length == 1 && ((MessageType)resultData[Const.MessageTypeIndex] == MessageType.ChangeTimeout));
}
return(result);
}
public void DataArrayGetBits()
{
ProtocolArray array = new ProtocolArray(new byte[] { 165, 189, 219, 231 }); //10100101, 10111101, 11011011, 11100111
Assert.AreEqual(array.GetBits(0, 4), (uint)5);
Assert.AreEqual(array.GetBits(0, 4), (uint)10);
Assert.AreEqual(array.GetBits(0, 4), (uint)13);
Assert.AreEqual(array.GetBits(0, 4), (uint)11);
Assert.AreEqual(array.GetBits(0, 4), (uint)11);
Assert.AreEqual(array.GetBits(0, 4), (uint)13);
Assert.AreEqual(array.GetBits(0, 4), (uint)7);
Assert.AreEqual(array.GetBits(0, 4), (uint)14);
array.Position = 0;
Assert.AreEqual(array.GetBits(0, 10), (uint)421);
Assert.AreEqual(array.GetBits(0, 10), (uint)751);
Assert.AreEqual(array.GetBits(0, 10), (uint)637);
Assert.AreEqual(array.GetBits(0, 2), (uint)3);
}
public virtual void UpdateEncoderRequest(ProtocolArray requestData)
{
if (EncoderOffset != 0)
{
if (0 == encoderOffsetCache)
{
encoderOffsetBits = ProtocolArray.SignificantBits((uint)Math.Abs(EncoderOffset));
encoderOffsetMask = (byte)((encoderOffsetBits << 1) + 1);
encoderOffsetBits++;
encoderOffsetCache = (Math.Abs(EncoderOffset) << 1) | -(Math.Sign(EncoderOffset) >> 1); //odd numbers are negative speed/backward direction
}
requestData.SetBits(1, 6, encoderOffsetMask);
requestData.SetBits(1, encoderOffsetBits, (uint)encoderOffsetCache);
}
else
{
requestData.SetBits(1, 1, 0);
}
}
public override void UpdateSensorResponse(ProtocolArray responseData)
{
int previous = RawValue;
if (SensorType == SensorType.ULTRASONIC_CONT)
{
base.UpdateSensorResponse(responseData);
}
else //SensorType.ULTRASONIC_SS
{
RawValue = (int)responseData.GetBits(1, 8);
}
if (Math.Abs(previous - Distance) >= Threshold)
{
this.OnChangedEventHandler(new UltraSonicSensorEventArgs()
{
Distance = Distance
});
}
}
public override void UpdateSensorResponse(ProtocolArray responseData)
{
bool state = Pressed;
if (SensorType == SensorType.EV3_TOUCH_0)
{
RawValue = (int)responseData.GetBits(1, 16);
}
else
{
base.UpdateSensorResponse(responseData);
}
Pressed = (RawValue >= threshold);
if (state != Pressed)
{
this.OnChangedEventHandler(new TouchSensorEventArgs()
{
Pressed = this.Pressed
});
if (Pressed)
{
if (null != OnPressed)
{
Task.Run(() => OnPressed(this, new SensorEventArgs()));
}
}
else
{
if (null != OnReleased)
{
Task.Run(() => OnReleased(this, new SensorEventArgs()));
}
}
}
base.UpdateSensorResponse(responseData);
}
public override void UpdateSensorRequest(ProtocolArray requestData)
{
base.UpdateSensorRequest(requestData);
int arduinoPort = (int)SensorPort.ArduinoPort();
if ((SensorType == SensorType.I2C) ||
(SensorType == SensorType.I2C_9V) ||
(SensorType == SensorType.ULTRASONIC_CONT))
{
foreach (I2CData device in I2CData)
{
if ((device.Setting & Const.BIT_I2C_SAME) != Const.BIT_I2C_SAME)
{
requestData.SetBits(1, 4, (uint)device.WriteBytes);
requestData.SetBits(1, 4, (uint)device.ReadBytes);
for (int out_byte = 0; out_byte < device.WriteBytes; out_byte++)
{
requestData.SetBits(1, 8, (uint)device.WriteData[out_byte]);
}
}
}
}
}
public virtual void UpdateSensorRequest(ProtocolArray requestData)
{
//nothing to do here
}
public override TimeSpan?InitializeSensorRequest(ProtocolArray requestData)
{
//ColorSensor needs some extra time for setup (due to calibration?)
return((base.InitializeSensorRequest(requestData)).Max(setupTime));
}
public void DataArraySignificantBitsTest0()
{
Assert.AreEqual(0, ProtocolArray.SignificantBits(uint.MinValue));
}
public void DataArraySignificantBitsTestMaxUInt()
{
Assert.AreEqual(32, ProtocolArray.SignificantBits(uint.MaxValue));
}
public void DataArrayPositionInitialTest()
{
ProtocolArray array = new ProtocolArray();
Assert.AreEqual(0, array.Position);
}
public virtual void UpdateSensorResponse(ProtocolArray responseData)
{
//TODO: any specific implementations should be done in dedicated sensor classes
switch (SensorType)
{
case SensorType.RAW:
//this is 0 value, LIGHT_OFF is 0 as well
//case SensorType.LIGHT_OFF:
RawValue = (int)responseData.GetBits(1, 10);
break;
case SensorType.LIGHT_ON:
throw new NotImplementedException();
case SensorType.RCX_LIGHT:
throw new NotImplementedException();
case SensorType.EV3_INFRARED_M2:
case SensorType.EV3_GYRO_M3:
case SensorType.EV3_COLOR_M3:
RawValue = (int)responseData.GetBits(1, 32);
break;
case SensorType.EV3_US_M0:
case SensorType.EV3_US_M1:
case SensorType.EV3_US_M2:
case SensorType.EV3_US_M3:
case SensorType.EV3_US_M4:
case SensorType.EV3_US_M5:
case SensorType.EV3_US_M6:
case SensorType.EV3_COLOR_M0:
case SensorType.EV3_COLOR_M1:
case SensorType.EV3_COLOR_M2:
case SensorType.EV3_COLOR_M4:
case SensorType.EV3_COLOR_M5:
case SensorType.EV3_GYRO_M0:
case SensorType.EV3_GYRO_M1:
case SensorType.EV3_GYRO_M2:
case SensorType.EV3_GYRO_M4:
case SensorType.EV3_INFRARED_M0:
case SensorType.EV3_INFRARED_M1:
case SensorType.EV3_INFRARED_M3:
case SensorType.EV3_INFRARED_M4:
case SensorType.EV3_INFRARED_M5:
RawValue = (int)responseData.GetBits(1, 16);
//# EV3 Gyro Mode 0, Adjust sign
if (SensorType == SensorType.EV3_GYRO_M0)
{
if (RawValue >= short.MaxValue) //# Negative number. This seems to return a 2 byte number.
{
RawValue = RawValue - 65535;
}
}
//# EV3 Gyro Mode 1, Adjust sign
else if (SensorType == SensorType.EV3_GYRO_M1)
{
if (RawValue >= short.MaxValue) // # Negative number. This seems to return a 2 byte number.
{
RawValue = RawValue - 65535;
}
}
break;
case SensorType.EV3_TOUCH_DEBOUNCE:
case SensorType.COLOR_RED:
case SensorType.COLOR_GREEN:
case SensorType.COLOR_BLUE:
case SensorType.COLOR_NONE:
default:
RawValue = (int)responseData.GetBits(1, 10);
break;
}
}
/// <summary>
/// poll the BrickPi for updates on sensor and encoder data, and send motor command
/// </summary>
/// <returns></returns>
private async Task <bool> BrickPiUpdateValues()
{
ProtocolArray dataArray;
int retry = 0;
foreach (Arduino arduino in EnumExtension <Arduino> .All())
{
//Fill the header of buffer for communication
dataArray = new ProtocolArray();
dataArray[Const.MessageTypeIndex] = (int)MessageType.Datagram;
// motors encoder offset
foreach (MotorPort motorPort in arduino.MotorPorts())
{
motors[motorPort].UpdateEncoderRequest(dataArray);
}
// motors speed and direction
foreach (MotorPort motorPort in arduino.MotorPorts())
{
motors[motorPort].UpdateVelocityRequest(dataArray);
}
// sensors
foreach (SensorPort sensorPort in arduino.SensorPorts())
{
sensors[sensorPort].UpdateSensorRequest(dataArray);
}
int bytes = 1 + dataArray.Bytes;
#if DEBUGMESSAGES
Debug.WriteLine($"{nameof(this.BrickPiUpdateValues)} - {arduino.ToString()}");
#endif
byte[] resultData = await BrickPiTxAndRx(arduino, bytes, dataArray.Data).ConfigureAwait(false);
if (resultData == null)
{
continue;
}
if (resultData == null || resultData.Length <= 1 || (MessageType)resultData[Const.MessageTypeIndex] != MessageType.Datagram)
{
Debug.WriteLine($"Error Updating values: {BitConverter.ToString(resultData ?? System.Text.Encoding.Unicode.GetBytes("No Data Received"))}");
await Task.Delay(100).ConfigureAwait(false);
if (retry++ < 3)
{
continue;
}
else
{
return(false);
}
}
dataArray = new ProtocolArray(resultData);
// motors
// need to get number of bits to be used for each encoder first
byte[] encoderBits = new byte[] { (byte)dataArray.GetBits(1, 5), (byte)dataArray.GetBits(1, 5) };
foreach (MotorPort motorPort in arduino.MotorPorts())
{
motors[motorPort].UpdateResponse(dataArray, encoderBits[(byte)motorPort % 2]);
}
// sensors
foreach (SensorPort sensorPort in arduino.SensorPorts())
{
sensors[sensorPort].UpdateSensorResponse(dataArray);
}
}
return(true);
}
