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 static MotorPort[] MotorPorts(this Arduino arduino)
{
switch (arduino)
{
case Arduino.Arduino1:
return(new MotorPort[] { BrickPi.Uwp.Base.MotorPort.Port_MA, BrickPi.Uwp.Base.MotorPort.Port_MB });
case Arduino.Arduino2:
return(new MotorPort[] { BrickPi.Uwp.Base.MotorPort.Port_MC, BrickPi.Uwp.Base.MotorPort.Port_MD });
default:
return(EnumExtension <MotorPort> .All());
}
}
public static SensorPort[] SensorPorts(this Arduino arduino)
{
switch (arduino)
{
case Arduino.Arduino1:
return(new SensorPort[] { BrickPi.Uwp.Base.SensorPort.Port_S1, BrickPi.Uwp.Base.SensorPort.Port_S2 });
case Arduino.Arduino2:
return(new SensorPort[] { BrickPi.Uwp.Base.SensorPort.Port_S3, BrickPi.Uwp.Base.SensorPort.Port_S4 });
default:
return(EnumExtension <SensorPort> .All());
}
}
/// <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);
}
/// <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);
}