public void putCommand(AICommand command)
{
aiCommands.Add(command);
}
private void aiDrive(List <Robot> robots, List <Ball> balls)
{
if (this.previousPosition.Equals(location))
{
stuckCount++;
}
else
{
stuckCount = 0;
}
bool moved = false;
rotation = (float)UTIL.normalizeDirection(rotation);
float turnAxis, strafeAxis, powerAxis;
bool fire = false;
turnAxis = strafeAxis = powerAxis = 0f;
AICommand command = aiHandler.get();
if (!command.Equals(previous))
{
cycles = 0;
}
cycles++;
if (cycles > command.getTimeout())
{
aiHandler.move();
moved = true;
command = aiHandler.get();
cycles = 0;
}
Vector2 target = Vector2.Zero;
if (command.getType() == AICommand.driveCommand)
{
target = (Vector2)command.getValue();
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.normalizeDirection(UTIL.getDirectionTward(location, target));
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (UTIL.distance(target, location) < 35 && !moved)
{
moved = true;
aiHandler.move();
}
}
else if (command.getType() == AICommand.fireCommand)
{
double goal = 0.0;
if (color.Equals(Color.Blue))
{
goal = Math.PI;
}
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
powerAxis = (aiTurnPID.isDone()) ? -0.3f : 0.25f;
fire = aiTurnPID.isDone();
if (activeBall == null && !moved)
{
aiHandler.move();
moved = true;
}
}
else if (command.getType() == AICommand.passCommand)
{
double goal = 0.0;
double minDistance = double.MaxValue;
foreach (Robot r in robots)
{
if (!r.Equals(this) && r.getColor().Equals(color))
{
if (UTIL.distance(location, r.getLocation()) < minDistance)
{
minDistance = UTIL.distance(location, r.getLocation());
goal = UTIL.getDirectionTward(location, r.getLocation());
}
}
}
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
fire = aiTurnPID.isDone();
if (!moved && (activeBall == null || activeBall.getAssistBonus() >= 10))
{
aiHandler.move();
}
}
else if (command.getType() == AICommand.positionCommand)
{
Vector2 offSet = (Vector2)command.getValue();
Vector2 ballCoordinate = Vector2.Zero;
double minDistance = double.MaxValue;
foreach (Ball b in balls)
{
if (b.getColor().Equals(color))
{
if (UTIL.distance(location, b.getLocation()) < minDistance && ((offSet.Equals(Vector2.Zero)) ? b.getIsFree() : !b.getIsFree()))
{
target = b.getLocation() + offSet;
if (primaryZone == RedPrimary)
{
if (target.X > redZone * widthScale - zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
else if (primaryZone == WhitePrimary)
{
if (target.X > blueZone * widthScale - zoneBleed * widthScale && target.X < redZone * widthScale + zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
else if (primaryZone == BluePrimary)
{
if (target.X < blueZone * widthScale + zoneBleed * widthScale)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
}
}
}
target = ballCoordinate + offSet;
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.getDirectionTward(location, target);
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (!moved && activeBall != null || ballCoordinate.Equals(Vector2.Zero))
{
moved = true;
aiHandler.move();
}
}
else if (command.getType() == AICommand.defenseCommand)
{
Vector2 offSet = Vector2.Zero;
Vector2 ballCoordinate = Vector2.Zero;
double minDistance = double.MaxValue;
foreach (Ball b in balls)
{
if (!b.getColor().Equals(color))
{
if (UTIL.distance(location, b.getLocation()) < minDistance)
{
ballCoordinate = b.getLocation();
minDistance = UTIL.distance(ballCoordinate, location);
}
}
}
target = ballCoordinate + offSet;
powerAxis = (float)aiDrivePID.calcPID(UTIL.distance(location, target));
double goal = UTIL.getDirectionTward(location, target);
if (Math.Abs(goal + 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal += 2 * Math.PI;
}
if (Math.Abs(goal - 2 * Math.PI - rotation) < Math.Abs(goal - rotation))
{
goal -= 2 * Math.PI;
}
aiTurnPID.setDesiredValue(goal);
turnAxis = (float)aiTurnPID.calcPID(rotation);
if (activeBall != null && !moved)
{
moved = true;
aiHandler.move();
}
}
if (!target.Equals(Vector2.Zero) && !moved)
{
if (primaryZone == RedPrimary)
{
if (target.X < redZone * widthScale - zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
else if (primaryZone == WhitePrimary)
{
if (target.X < blueZone * widthScale - zoneBleed * widthScale || target.X > redZone * widthScale + zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
else if (primaryZone == BluePrimary)
{
if (target.X > blueZone * widthScale + zoneBleed * widthScale)
{
aiHandler.move();
moved = true;
}
}
}
if (!moved || fire)
{
drive(robots, balls, turnAxis, powerAxis, strafeAxis);
}
else
{
drive(robots, balls, 0, 0, 0);
}
if (stuckCount >= 200)
{
turnAxis = -0.5f;
powerAxis = -1.0f;
strafeAxis = 0.0f;
drive(robots, balls, turnAxis, powerAxis, strafeAxis);
}
if (activeBall != null && fire)
{
activeBall.launch(new Vector3(launchPower * (float)Math.Cos(rotation) + velocity.X, launchPower * (float)Math.Sin(rotation) + velocity.Y, 2f));
activeBall = null;
launch.Play();
}
previous = command;
}