public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
referenceSpeed = speed;
referenceHeading = heading;
if (Math.Abs(robotPosition.X) > robotMaxPosition || Math.Abs(robotPosition.Y) > robotMaxPosition)
{
returnToCenter.calculateNextMove(robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
}
}
public override void calculateNextMove(DoublePoint robotPosition, double speed, DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out DoublePoint referenceHeading)
{
if (!pointCalculated)
{
pointAcrossField = calculatePointAcrossField(robotPosition, neighbors);
nextPoint = pointAcrossField;
startPoint = robotPosition;
pointCalculated = true;
pointReached = false;
}
if (robotPosition.DistanceTo(nextPoint) < 20)
{
pointReached = true;
referenceHeading = heading;
referenceSpeed = 0;
}
else
{
goToPoint.calculateNextMove(nextPoint, robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
collisionStrategy.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
}
}
public void calculateNextMove(CollisionArea collisionArea, AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
{
bool go = true;
if (ControlStrategy.isAtHeading(collisionArea, robotPosition, heading))
{
int distance = collisionArea.distance2Middle(robotPosition);
if (queue.Count == 0)
{
foreach (Robot neighbor in neighbors)
{
if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
{
if (distance > collisionArea.distance2Robot(neighbor))
{
queue.Add(neighbor.getID());
go = false;
}
}
}
}
else
{
foreach (Robot neighbor in neighbors)
{
if (ControlStrategy.isAtHeading(collisionArea, neighbor.getPosition(), neighbor.getHeading()))
{
foreach (int ID in queue)
{
if (ID == neighbor.getID())
{
go = false;
break;
}
}
}
}
}
foreach (Robot neighbor in neighbors)
{
if (collisionArea.intersects(neighbor.getPosition()))
{
go = false;
break;
}
}
}
else
{
queue.Clear();
}
if (go)
{
referenceSpeed = speed;
referenceHeading = heading;
}
else
{
stopStrategy.calculateNextMove(collisionArea.getNearestPoint(robotPosition), Program.robotRadius, robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
}
corner1 = collisionArea.getHeadingEdges(robotPosition, heading)[0];
corner2 = collisionArea.getHeadingEdges(robotPosition, heading)[1];
this.robotPosition = robotPosition;
}
/*
* This function firstly calculates the closest point, drives towards it and finally starts the path.
*/
public override void calculateNextMove(AForge.DoublePoint robotPosition, double speed, AForge.DoublePoint heading, List <Robot> neighbors, out double referenceSpeed, out AForge.DoublePoint referenceHeading)
{
if (!onLine)
{
pointCount = 0;
double distance = robotPosition.DistanceTo(points[pointCount]);
for (int i = 0; i < points.Count(); i++)
{
if (distance > points[i].DistanceTo(robotPosition))
{
pointCount = i;
distance = points[pointCount].DistanceTo(robotPosition);
}
}
if (distance < closeUpLimit)
{
onLine = true;
}
else
{
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
return;
}
}
if (onLine)
{
double distance = robotPosition.DistanceTo(points[pointCount]);
if (distance <= closeUpLimit)
{
if (direction == 1)
{
pointCount = (pointCount >= points.Count() - 1 ? 0 : pointCount + direction);
}
else
{
pointCount = (pointCount >= 1 ? points.Count() - 1 : pointCount + direction);
//pointCount = (pointCount > 1 ? pointCount - 1 : points.Count() - 1); //Shouldn't it be this?
}
}
else
{
onLine = false;
}
goToPoint.calculateNextMove(points[pointCount], robotPosition, speed, heading, neighbors, out referenceSpeed, out referenceHeading);
if (fifoStrategy != null)
{
fifoStrategy.calculateNextMove(robotPosition, referenceSpeed, referenceHeading, neighbors, out referenceSpeed, out referenceHeading);
}
return;
}
//referenceSpeed = speed; //uncomment if using m3pi robots
//$$$$$Changes/Additions made for RC cars$$$$$//
referenceSpeed = Program.testSpeed;
//$$$$$$$$$$//
referenceHeading = heading;
}