private void button1_Click(object sender, EventArgs e)
{
robot.ReadLaserValues();
Console.WriteLine("Robot: {0} {1}", robot.Center.x, robot.Center.y);
Console.WriteLine("Robot: {0} {1} {2} {3}", robot.LL, robot.LF, robot.RF, robot.RR);
CreatePathForm createPathForm = new CreatePathForm();
createPathForm.FormClosing += (o, form) =>
{
var src = new Point(Int32.Parse(createPathForm.srcXTextBox.Text), Int32.Parse(createPathForm.srcYTextBox.Text));
LocationApproximator locationApproximator = new LocationApproximator();
locationApproximator.SetUp(map);
Point loc = locationApproximator.GetLocation((int)(src.x - R / 2), (int)(src.y - R / 2), R, R, 2, robot.LL, robot.LF, robot.RR, robot.RF);
robot.Center = loc;
Console.WriteLine("Robot: {0} {1}", robot.Center.x, robot.Center.y);
Console.WriteLine("Robot: {0} {1} {2} {3}", robot.LL, robot.LF, robot.RF, robot.RR);
Render();
};
createPathForm.Show();
Render();
}
public void Start(Point src, string name, Orientation orientation)
{
route = new RoutePlanner(robot, map);
StartRoutingTo(src, name);
this.orientations.Add(orientation);
this.names.Add(name);
}
public Point TranslateLocalVector(Point p)
{
Point t = new Point();
if (robot.Orientation == Orientation.B)
{
return(null);
}
if (robot.Orientation == Orientation.N)
{
t.x = p.x;
t.y = -p.y;
}
else if (robot.Orientation == Orientation.E)
{
t.x = p.y;
t.y = p.x;
}
else if (robot.Orientation == Orientation.S)
{
t.y = p.y;
t.x = -p.x;
}
else if (robot.Orientation == Orientation.W)
{
t.x = -p.y;
t.y = -p.x;
}
return(t);
}
public void Start(Point src, List <String> names, List <Orientation> orientations)
{
route = new RoutePlanner(robot, map);
StartRoutingTo(src, names[0]);
this.orientations.AddRange(orientations);
this.names.AddRange(names);
// var rec = FindStageRegion(name);
// Point p = FindDestinationPoint(Rectangle.Round(rec));
// route.Start(src, p);
// this.orientation = orientation;
}
public void StartRoutingTo(Point src, string name)
{
var rec = FindStageRegion(name);
Point p = FindDestinationPoint(Rectangle.Round(rec));
if (p == null)
{
p = new Point(rec.X, rec.Y);
}
log.Info("Location For Working Found" + p.x + "," + p.y);
route.Start(src, p);
}
private void navigateBtn_Click(object sender, EventArgs e)
{
CreatePathForm createPathForm = new CreatePathForm();
createPathForm.map = map;
createPathForm.FormClosing += (o, form) =>
{
route = new RoutePlanner(robot, map);
var src = new Point(Int32.Parse(createPathForm.srcXTextBox.Text), Int32.Parse(createPathForm.srcYTextBox.Text));
var dest = new Point(Int32.Parse(createPathForm.dstXtextBox.Text), Int32.Parse(createPathForm.dstYTextBox.Text));
route.Start(src, dest);
Timer1.Enabled = true;
};
createPathForm.Show();
}
private void bntBtn_Click(object sender, EventArgs e)
{
/// robot.ReadLaserValues();
// robot.Rotate(90);
// return;
//Console.WriteLine("Robot: {0} {1}", robot.Center.x, robot.Center.y);
//Console.WriteLine("Robot: {0} {1} {2} {3}", robot.LL, robot.LF, robot.RF, robot.RR);
CreatePathForm createPathForm = new CreatePathForm();
createPathForm.FormClosing += (o, form) =>
{
var src = new Point(Int32.Parse(createPathForm.srcXTextBox.Text), Int32.Parse(createPathForm.srcYTextBox.Text));
// LocationApproximator locationApproximator = new LocationApproximator();
// locationApproximator.SetUp(map);
// Point loc = locationApproximator.GetLocation((int)(src.x - R / 2), (int)(src.y - R / 2), R, R, 2, robot.LL, robot.LF, robot.RR, robot.RF);
// robot.Center = loc;
Console.WriteLine("Robot: {0} {1}", robot.Center.x, robot.Center.y);
Console.WriteLine("Robot: {0} {1} {2} {3}", robot.LL, robot.LF, robot.RF, robot.RR);
bnt = new BNT(map, robot);
List <String> names = new List <string>(createPathForm.dstXtextBox.Text.Split(' '));
List <String> oo = new List <string>(createPathForm.dstYTextBox.Text.Split(' '));
List <Orientation> orientations = oo.ConvertAll(input =>
{
Orientation orientation;
Enum.TryParse <Orientation>(input, out orientation);
return(orientation);
});
// bnt.TEST(src, names, orientations);
bnt.Start(src, names, orientations);
Timer1.Enabled = true;
Render();
};
createPathForm.Show();
Render();
}
public Point FindEntryVector()
{
Point e = null;
foreach (var o in map.obstacles)
{
if (o.Type == WorldObjectType.Entry)
{
if (o.Left)
{
e = new Point(1, 0);
}
else if (o.Right)
{
e = new Point(-1, 0);
}
else if (o.Down)
{
e = new Point(0, -1);
}
else if (o.Up)
{
e = new Point(0, 1);
}
}
if (e != null)
{
log.Info("Found Entry : " + e.ToString());
}
else
{
log.Error("No Entry Found");
}
}
return(e);
}
public bool TEST(Point src, List <String> names, List <Orientation> orientations)
{
PathFinder pf = new PathFinder();
pf.LoadInMap(map);
for (int i = 0; i < names.Count; i++)
{
var name = names[i];
var rec = FindStageRegion(name);
Point p = FindDestinationPoint(Rectangle.Round(rec));
if (p == null)
{
log.Info("No Location Found" + p.x + "," + p.y);
p = new Point(rec.X, rec.Y);
}
log.Info("Location For Working Found" + p.x + "," + p.y);
pf.setSrc((int)src.x, (int)src.y);
pf.setDst((int)p.x, (int)p.y);
pf.findPath();
var path = pf.getPath();
if (path.Count > 1)
{
log.Info("Path found for Target:" + name);
}
else
{
log.Error("Path Not Found for Target:" + name);
return(false);
}
}
return(true);
}
