bradybod-usu/robot_intelligence_final_project
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

I think its done

Browse Source
This commit is contained in:
Brady Bodily
2020-12-16 17:19:25 -07:00
parent a792de99f3
commit e335ce3c68
34 changed files with 5679 additions and 927 deletions
Download Patch File Download Diff File Expand all files Collapse all files

62
ConsoleApp/PathPlanners/PathPlanner.cs
View File

@@ -11,49 +11,49 @@ namespace ConsoleApp.PathPlanners
{
public Queue<ICell> GenerateOptimalSquarePath(ISquareMap map, IVehicle vehicle)
{
var path = new Queue<ICell>();
var myCell = map.StartingCell;
var currentPostion = vehicle.CurrentSquareCell;
var currentHeading = GlobalDirection.East;
var finished = false;
var width_cm = (double)(vehicle.DetectorRadius) * 100 * 2;
var swathOffset = (int)Math.Floor((decimal) (width_cm) / 25) + 1;
var currentHeading = GlobalDirection.North;
var path = new List<ICell>();
while (!finished)
{
var availableMoves = map.PossibleMoves(myCell);
if (availableMoves.Contains(GlobalDirection.North) && currentHeading == GlobalDirection.North &&
myCell.Y != map.Height)
//Heading East
if (currentHeading == GlobalDirection.East)
{
path.Enqueue(map.GetCell(myCell.X, myCell.Y + 1));
myCell = map.GetCell(myCell.X, myCell.Y + 1);
path.AddRange(
map.GetShortestPath(currentPostion, map.GetCell(map.Width - 1, currentPostion.Y)));
currentPostion = new Cell(map.Width - 1, currentPostion.Y);
}
else if (availableMoves.Contains(GlobalDirection.South) && currentHeading == GlobalDirection.South && myCell.Y != 0)
//Heading West
else if (currentHeading == GlobalDirection.West)
{
path.Enqueue(map.GetCell(myCell.X, myCell.Y - 1));
myCell = map.GetCell(myCell.X, myCell.Y - 1);
path.AddRange(
map.GetShortestPath(currentPostion, map.GetCell(0, currentPostion.Y)));
currentPostion = new Cell(0, currentPostion.Y);
}
else
//Check for finish
if (currentPostion.Y + (vehicle.DetectorRadius * 2) >= map.Height)
{
if (myCell.X + swathOffset >= map.Width)
{
finished = true;
}
else
{
for (int i = myCell.X; i < myCell.X + swathOffset; i++)
{
path.Enqueue(map.GetCell(i, myCell.Y));
}
myCell = map.GetCell(myCell.X+swathOffset, myCell.Y);
if (currentHeading == GlobalDirection.North)
currentHeading = GlobalDirection.South;
else if (currentHeading == GlobalDirection.South)
currentHeading = GlobalDirection.North;
}
finished = true;
continue;
}
//Move up X Axis
path.AddRange(
map.GetShortestPath(
currentPostion,
map.GetCell(currentPostion.X, currentPostion.Y + vehicle.DetectorRadius * 2 - 1)));
currentPostion = new Cell(currentPostion.X, currentPostion.Y + (vehicle.DetectorRadius * 2 - 1));
if (currentHeading == GlobalDirection.East)
currentHeading = GlobalDirection.West;
else
currentHeading = GlobalDirection.East;
}
return path;
return new Queue<ICell>(path);
}
public Queue<Coordinate2D> GenerateOptimalHexPath(IHexMap hexMap, IVehicle vehicle)