private async Task<bool> BrickPiUpdateValues()
{
//If setup is needed, then first setup the sensors
await SetupSensors();
//if need to change the timeout of the brick
if (needTimeout)
{
BrickPiSetTimeout().Wait();
needTimeout = false;
}
DataArray dataArray = new DataArray();
int Retried = 0;
bool ret = false;
int idxArduino = 0;
while (idxArduino < 2)
{
if (!ret)
Retried = 0;
//Fill the header of buffer for communication
// BYTE_MSG_TYPE, position 0, Message to send
for (int ii = 0; ii < dataArray.myArray.Length; ii++)
dataArray.myArray[ii] = 0;
dataArray.myArray[BYTE_MSG_TYPE] = MSG_TYPE_VALUES;
dataArray.Bit_Offset = 0;
// This second part is specific to motors encoders.
for (int idxPort = 0; idxPort < 2; idxPort++)
{
int port = (idxArduino * 2) + idxPort;
if (BrickPi.Motor[port].EncoderOffset != 0)
{
int Temp_Value = BrickPi.Motor[port].EncoderOffset;
dataArray.AddBits(1, 0, 1, 1);
int Temp_ENC_DIR = 0;
if (Temp_Value < 0)
{
Temp_ENC_DIR = 1;
Temp_Value *= -1;
}
byte Temp_BitsNeeded = (byte)(dataArray.BitsNeeded(Temp_Value) + 1);
dataArray.AddBits(1, 0, 5, Temp_BitsNeeded);
Temp_Value *= 2;
Temp_Value |= Temp_ENC_DIR;
dataArray.AddBits(1, 0, Temp_BitsNeeded, Temp_Value);
}
else
dataArray.AddBits(1, 0, 1, 0);
}
// This third part is specific to motors speed and direction
for (int idxPort = 0; idxPort < 2; idxPort++)
{
int port = (idxArduino * 2) + idxPort;
int speed = BrickPi.Motor[port].Speed;
int direc = 0;
if (speed < 0)
{
direc = 1;
speed *= -1;
}
if (speed > 255)
speed = 255;
dataArray.AddBits(1, 0, 10, ((((speed & 0xFF) << 2) | (direc << 1) | (BrickPi.Motor[port].Enable & 0x01)) & 0x3FF));
}
// This part is specific to I2C sensors
for (int idxPort = 0; idxPort < 2; idxPort++)
{
int port = (idxArduino * 2) + idxPort;
if ((BrickPi.Sensor[port].Type == BrickSensorType.I2C) ||
(BrickPi.Sensor[port].Type == BrickSensorType.I2C_9V) ||
(BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT))
{
for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
{
if ((BrickPi.Sensor[port].Settings[device] & BIT_I2C_SAME) != BIT_I2C_SAME)
{
dataArray.AddBits(1, 0, 4, BrickPi.I2C[port].Write[device]);
dataArray.AddBits(1, 0, 4, BrickPi.I2C[port].Read[device]);
for (int out_byte = 0; out_byte < BrickPi.I2C[port].Write[device]; out_byte++)
dataArray.AddBits(1, 0, 8, BrickPi.I2C[port].Out[device].InOut[out_byte]);
}
// ITODO don't understadn why??? so letting it as it is:
device += 1;
}
}
}
byte[] InArray = null;
try
{
int tx_bytes = (((dataArray.Bit_Offset + 7) / 8) + 1); // #eq to UART_TX_BYTES
BrickPiTx(BrickPi.Address[idxArduino], tx_bytes, dataArray.myArray);
//wait for answer
// IMPORTANT: in theory, waiting time if 7.5 ms but it does create problems
// so it is working fine with a 20 ms timeout.
InArray = await BrickPiRx(15); //theory 7.5 ms
}
catch (Exception ex)
{
Debug.WriteLine(string.Format("Error reading/writing: {0}", ex.Message));
continue;
}
if (InArray == null)
continue;
//Send the data
// first part is about encoders
BrickPi.Motor[(idxArduino * 2) + (int)BrickPortMotor.PORT_A].EncoderOffset = 0;
BrickPi.Motor[(idxArduino * 2) + (int)BrickPortMotor.PORT_B].EncoderOffset = 0;
byte[] OutArray;
//check if we got the right answer to the question
if (!CheckRetMessage(InArray, MSG_TYPE_VALUES, out OutArray))
{
Debug.WriteLine(String.Format("BrickPi Error reading value in Updating values"));
if (Retried < 2)
{
ret = true;
Retried += 1;
continue;
}
}
dataArray.myArray = OutArray;
ret = false;
dataArray.Bit_Offset = 0;
// number of bits to be used for encoders
List<byte> Temp_BitsUsed = new List<byte>();
Temp_BitsUsed.Add((byte)dataArray.GetBits(1, 0, 5));
Temp_BitsUsed.Add((byte)dataArray.GetBits(1, 0, 5));
for (int idxPort = 0; idxPort < 2; idxPort++)
{
long Temp_EncoderVal = dataArray.GetBits(1, 0, Temp_BitsUsed[idxPort]);
if ((Temp_EncoderVal & 0x01) == 1)
{
Temp_EncoderVal /= 2;
BrickPi.Motor[idxPort + idxArduino * 2].Encoder = (int)(Temp_EncoderVal) * -1;
}
else
BrickPi.Motor[idxPort + idxArduino * 2].Encoder = (int)(Temp_EncoderVal / 2);
}
// This is the part with sensors
for (int idxPort = 0; idxPort < 2; idxPort++)
{
int port = idxPort + (idxArduino * 2);
switch (BrickPi.Sensor[port].Type)
{
case BrickSensorType.SENSOR_RAW:
//this is 0 value, LIGHT_OFF is as well 0
//case BrickSensorType.LIGHT_OFF:
break;
case BrickSensorType.LIGHT_ON:
break;
case BrickSensorType.TOUCH:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 1);
break;
case BrickSensorType.ULTRASONIC_SS:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 8);
break;
case BrickSensorType.RCX_LIGHT:
break;
case BrickSensorType.COLOR_FULL:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 3);
BrickPi.Sensor[port].Array[INDEX_BLANK] = (int)dataArray.GetBits(1, 0, 10);
BrickPi.Sensor[port].Array[INDEX_RED] = (int)dataArray.GetBits(1, 0, 10);
BrickPi.Sensor[port].Array[INDEX_GREEN] = (int)dataArray.GetBits(1, 0, 10);
BrickPi.Sensor[port].Array[INDEX_BLUE] = (int)dataArray.GetBits(1, 0, 10);
break;
case BrickSensorType.ULTRASONIC_CONT:
case BrickSensorType.I2C:
case BrickSensorType.I2C_9V:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, BrickPi.I2C[port].Devices);
for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
{
if ((BrickPi.Sensor[port].Value & (0x01 << device)) != 0)
for (int in_byte = 0; in_byte < BrickPi.I2C[port].Read[device]; in_byte++)
BrickPi.I2C[port].In[device].InOut[in_byte] = (int)dataArray.GetBits(1, 0, 8);
}
if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT)
{
if (((int)BrickPi.Sensor[port].Value & (0x01 << US_I2C_IDX)) != 0)
BrickPi.Sensor[port].Value = BrickPi.I2C[port].In[US_I2C_IDX].InOut[0];
else
BrickPi.Sensor[port].Value = -1;
}
break;
case BrickSensorType.EV3_INFRARED_M2:
case BrickSensorType.EV3_GYRO_M3:
case BrickSensorType.EV3_COLOR_M3:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
break;
case BrickSensorType.EV3_US_M0:
case BrickSensorType.EV3_US_M1:
case BrickSensorType.EV3_US_M2:
case BrickSensorType.EV3_US_M3:
case BrickSensorType.EV3_US_M4:
case BrickSensorType.EV3_US_M5:
case BrickSensorType.EV3_US_M6:
case BrickSensorType.EV3_COLOR_M0:
case BrickSensorType.EV3_COLOR_M1:
case BrickSensorType.EV3_COLOR_M2:
case BrickSensorType.EV3_COLOR_M4:
case BrickSensorType.EV3_COLOR_M5:
case BrickSensorType.EV3_GYRO_M0:
case BrickSensorType.EV3_GYRO_M1:
case BrickSensorType.EV3_GYRO_M2:
case BrickSensorType.EV3_GYRO_M4:
case BrickSensorType.EV3_INFRARED_M0:
case BrickSensorType.EV3_INFRARED_M1:
case BrickSensorType.EV3_INFRARED_M3:
case BrickSensorType.EV3_INFRARED_M4:
case BrickSensorType.EV3_INFRARED_M5:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
//# EV3 Gyro Mode 0, Adjust sign
if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M0)
{
if (BrickPi.Sensor[port].Value >= 32767) //# Negative number. This seems to return a 2 byte number.
BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
}
//# EV3 Gyro Mode 1, Adjust sign
else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M1)
{
if (BrickPi.Sensor[port].Value >= 32767) // # Negative number. This seems to return a 2 byte number.
BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
}
break;
case BrickSensorType.EV3_TOUCH_0:
BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
break;
case BrickSensorType.EV3_TOUCH_DEBOUNCE:
case BrickSensorType.TOUCH_DEBOUNCE:
case BrickSensorType.COLOR_RED:
case BrickSensorType.COLOR_GREEN:
case BrickSensorType.COLOR_BLUE:
case BrickSensorType.COLOR_NONE:
default:
BrickPi.Sensor[idxPort + (idxArduino * 2)].Value = (int)dataArray.GetBits(1, 0, 10);
break;
}
#region oldcode
//if (BrickPi.Sensor[port].Type == BrickSensorType.TOUCH)
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 1);
//else if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_SS)
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 8);
//else if (BrickPi.Sensor[port].Type == BrickSensorType.COLOR_FULL)
//{
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 3);
// BrickPi.Sensor[port].Array[INDEX_BLANK] = (int)dataArray.GetBits(1, 0, 10);
// BrickPi.Sensor[port].Array[INDEX_RED] = (int)dataArray.GetBits(1, 0, 10);
// BrickPi.Sensor[port].Array[INDEX_GREEN] = (int)dataArray.GetBits(1, 0, 10);
// BrickPi.Sensor[port].Array[INDEX_BLUE] = (int)dataArray.GetBits(1, 0, 10);
//}
//else if ((BrickPi.Sensor[port].Type == BrickSensorType.I2C) || (BrickPi.Sensor[port].Type == BrickSensorType.I2C_9V) || (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT))
//{
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, BrickPi.I2C[port].Devices);
// for (int device = 0; device < BrickPi.I2C[port].Devices; device++)
// {
// if ((BrickPi.Sensor[port].Value & (0x01 << device)) != 0)
// for (int in_byte = 0; in_byte < BrickPi.I2C[port].Read[device]; in_byte++)
// BrickPi.I2C[port].In[device].InOut[in_byte] = (int)dataArray.GetBits(1, 0, 8);
// }
//}
//else if ((BrickPi.Sensor[port].Type == BrickSensorType.EV3_COLOR_M3) || (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M3))
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
//else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_INFRARED_M2)
//{
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 32);
// if (BrickPi.Sensor[port].Value < 0)
// Debug.WriteLine("IR SENSOR RETURNED ERROR");
//}
//else if ((BrickPi.Sensor[port].Type >= BrickSensorType.EV3_US_M0) && (BrickPi.Sensor[port].Type <= BrickSensorType.EV3_INFRARED_M5 + 1))
// BrickPi.Sensor[port].Value = (int)dataArray.GetBits(1, 0, 16);
//else // #For all the light, color and raw sensors
// BrickPi.Sensor[ii + (i * 2)].Value = (int)dataArray.GetBits(1, 0, 10);
//if (BrickPi.Sensor[port].Type == BrickSensorType.ULTRASONIC_CONT)
//{
// if (((int)BrickPi.Sensor[port].Value & (0x01 << US_I2C_IDX)) != 0)
// BrickPi.Sensor[port].Value = BrickPi.I2C[port].In[US_I2C_IDX].InOut[0];
// else
// BrickPi.Sensor[port].Value = -1;
//}
//# EV3 Gyro Mode 0, Adjust sign
//if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M0)
//{
// if (BrickPi.Sensor[port].Value >= 32767) //# Negative number. This seems to return a 2 byte number.
// BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
// //#else: # Positive Number print str(gyro)
// //#######################
// //# EV3 Gyro Mode 1, Adjust sign
//}
//else if (BrickPi.Sensor[port].Type == BrickSensorType.EV3_GYRO_M1)
//{
// // # print "Gyro m1!"
// if (BrickPi.Sensor[port].Value >= 32767) // # Negative number. This seems to return a 2 byte number.
// BrickPi.Sensor[port].Value = BrickPi.Sensor[port].Value - 65535;
// // # else: # Positive Number print str(gyro)
//}
// # print BrickPi.SensorType[port]
#endregion
}
idxArduino++;
}
//if all went correctly, then ret should be false
return !ret;
}
