public Robot(JointMotorConfig jmc)
{
JointMotorConfig = jmc;
// Make bluetooth connections - if it is -1 don't make connection
nxtBricks = new NxtBrick[jmc.ComPorts.Length];
var connectTasks = new Task<NxtBrick>[jmc.ComPorts.Length];
for (int i = 0; i < jmc.ComPorts.Length; i++) {
var comPort = jmc.ComPorts[i];
if (comPort >= 0) {
int j = i; // Prevent access to modified closure
connectTasks[i] = new Task<NxtBrick>(() => ConnectBrick(j, jmc.LinkTypes[j], comPort));
connectTasks[i].Start();
}
}
Task.WaitAll(connectTasks.Where(a => a != null).ToArray());
Logger.Log("Connected all motors");
for (int i = 0; i < connectTasks.Length; i++) {
nxtBricks[i] = connectTasks[i] == null ? null : connectTasks[i].Result;
}
// Add joints
LeftArm = new Arm("Left", AddMotorToNxt(jmc.LeftShoulderAlong), AddMotorToNxt(jmc.LeftShoulderOut), AddMotorToNxt(jmc.LeftElbowAlong), AddMotorToNxt(jmc.LeftElbowOut));
RightArm = new Arm("Right", AddMotorToNxt(jmc.RightShoulderAlong), AddMotorToNxt(jmc.RightShoulderOut), AddMotorToNxt(jmc.RightElbowAlong), AddMotorToNxt(jmc.RightElbowOut));
}
private static void ThrowAggregateIfFaulted(Task<DocumentInfo>[] tasks)
{
var exceptions = tasks.Where<Task>(dt => dt.IsFaulted).Select(dt => dt.Exception).ToArray();
if (exceptions.Length > 0)
throw new AggregateException(exceptions);
}