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

5
ConsoleApp/PathPlanners/IReactivePathPlanner.cs
View File

@@ -8,5 +8,10 @@ namespace ConsoleApp.PathPlanners
{
Queue<Coordinate2D> ReactiveHexPath { get; }
void GenerateReactiveHexPath(IHexMap hexMap, Queue<Coordinate2D> optimalPath, Coordinate2D vehicleCurrentHexCell);
Queue<ICell> ReactiveSquarePath { get; }
void GenerateReactiveSquarePath(ISquareMap squareMap, Queue<ICell> optimalPath,
ICell vehicleCurrentSquareCell);
}
}

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)

34
ConsoleApp/PathPlanners/ReactivePathPlanner.cs
View File

@@ -7,12 +7,17 @@ namespace ConsoleApp.PathPlanners
public class ReactivePathPlanner : IReactivePathPlanner
{
private Queue<Coordinate2D> _reactiveHexPath;
private Queue<ICell> _reactiveSquarePath;
public Queue<Coordinate2D> ReactiveHexPath => _reactiveHexPath;
public Queue<ICell> ReactiveSquarePath => _reactiveSquarePath;
public ReactivePathPlanner()
{
_reactiveHexPath = new Queue<Coordinate2D>();
_reactiveSquarePath = new Queue<ICell>();
}
public void GenerateReactiveHexPath(IHexMap hexMap, Queue<Coordinate2D> optimalPath,
Coordinate2D vehicleCurrentHexCell)
@@ -39,6 +44,33 @@ namespace ConsoleApp.PathPlanners
_reactiveHexPath.Enqueue(node);
}
}
public void GenerateReactiveSquarePath(ISquareMap squareMap, Queue<ICell> optimalPath,
ICell vehicleCurrentSquareCell)
{
//Clean out old path
_reactiveSquarePath.Clear();
//If the optimal path is empty well we are done, otherwise pop 1
if (!optimalPath.TryDequeue(out ICell convergentPoint))
return;
//Dequeue until the cell is not blocked
while (squareMap.GetCell(convergentPoint.X, convergentPoint.Y).Blocked)
{
//if optimal path is empty were done and we can not clear around this position, otherwise
//lest pop another.
if (!optimalPath.TryDequeue(out convergentPoint))
return;
}
var map = (SquareMap)squareMap;
var reactivePath = map.GetShortestPath(vehicleCurrentSquareCell, convergentPoint);
foreach (var node in reactivePath)
{
_reactiveSquarePath.Enqueue(node);
}
}
}
}