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 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)
{
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()));
}
}
}
}
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);
}
}
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));
}
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 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 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);
}
/// <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);
}
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;
}
}
