void RobotReady(object o, EventArgs e)
{
lock (commands)
{
if (commands.Count > 0)
{
RouterCommand c = commands[0];
Console.WriteLine("Executing Command {0}", commands.Count);
commands.RemoveAt(0);
c.Execute(device);
}
else
{
finalPosition = device.GetPosition();
}
}
}
void RouterPositionUpdate(object o, EventArgs e)
{
if (o is IHardware)
{
IHardware hw = o as IHardware;
Vector3 position = hw.GetPosition();
//Console.WriteLine("Router New Position = " + position);
if ((lastPosition - position).Length > float.Epsilon)
{
previousPoints.Add(new PreviousPoint(DateTime.Now, lastPosition));
lastPosition = position;
}
}
}