public Point findPointToShootAt() {
// checks if it is clear to shoot at any of the points in the goal
for (Point point : goalPoints) {
if (!isInTheWay(ball.getCoors(), point, opponentRobot.getCoors())) return point;
}
return new Point(0, 0); // function that uses the point returned needs
// to check that it is not (0,0)
}
private boolean isClearToShoot(Point from) {
// if facing the other way, return false
if (shootingLeft && (nxt.getAngle() < Math.PI / 2 || nxt.getAngle() > 3 * Math.PI / 2)) {
// System.out.println("ERROR 1");
return false;
}
if (!shootingLeft && nxt.getAngle() > Math.PI / 2 && nxt.getAngle() < 3 * Math.PI / 2) {
// System.out.println("ERROR 2");
return false;
}
// if not pointing to the goal, return false
if (shootingLeft) {
int y = new Line(from, nxt.getAngle()).getYfromX(GOAL_LEFT);
if (y > GOAL_TOP || y < GOAL_BOTTOM) {
// System.out.println("ERROR 3");
return false;
}
} else {
int y = new Line(from, nxt.getAngle()).getYfromX(GOAL_RIGHT);
if (y > GOAL_TOP || y < GOAL_BOTTOM) {
// System.out.println("ERROR 4");
return false;
}
}
// if the other robot is behind just return true
if (shootingLeft && opponentRobot.getCoors().x > nxt.getCoors().x) return true;
if (!shootingLeft && opponentRobot.getCoors().x < nxt.getCoors().x) return true;
// 30 is size of the robot, sth larger should work better
if (new Line(from, nxt.getAngle()).distanceBetweenPointAndALine(opponentRobot.getCoors()) > 50)
return true;
// System.out.println("ERROR 5");
return false;
}
public void defend() {
getPitchInfo();
Line oppShootLine = new Line(ball.getCoors(), opponentRobot.getAngle());
int targetY = oppShootLine.getYfromX(nxt.getCoors().x);
System.out.println("YCood " + targetY);
System.out.println("OurCoord " + nxt.getCoors().y);
if (targetY > GOAL_TOP - 30) targetY = GOAL_TOP - 30;
if (targetY < GOAL_BOTTOM + 30) targetY = GOAL_BOTTOM + 30;
int targetX = shootingLeft ? GOAL_RIGHT - 30 : GOAL_LEFT + 30;
Point target = new Point(targetX, targetY);
printOnVisionFix(target, Color.CYAN);
int d = (int) Math.abs(target.y - nxt.getCoors().y);
int facingupordown = nxt.getAngle() > 0 && nxt.getAngle() < Math.PI ? 1 : -1;
int goingupordown = targetY > nxt.getCoors().y ? 1 : -1;
int s = 0;
if (d < 20) nxt.stop();
else if (d < 60) {
s = 200;
} else if (d < 90) {
s = 400;
} else {
s = 400;
}
nxt.adjustWheelSpeeds(facingupordown * goingupordown * s, facingupordown * goingupordown * s);
}
private void theLoop() {
while (true) {
// pause if gui asks to pause
while (!robotMoving) {
nxt.stop();
Tools.rest(100);
}
while (blockingMode && robotMoving) {
// this is the penalty mode that returns to normal game
defend();
Tools.rest(50);
}
// get rid of debug lines
vision.dropAllLines();
// update coordinates
getPitchInfo();
// if robot is by the wall, try to rotate
// 97-379 seem to be upper and lower limits for robots y value
int tmpy = ImageProcessor.barrelCorrected(nxt.getCoors()).y;
if (tmpy < MINYPOS || tmpy > MAXYPOS) {
System.out.println("im too close to wall");
boolean d = false;
int upordown = 0;
if (tmpy < MINYPOS) {
// d = ball.getCoors().x > nxt.getCoors().x;
d = !shootingLeft;
// which way should it be facing
upordown = nxt.getAngle() > 0 && nxt.getAngle() < Math.PI ? 1 : -1;
// by which side is it stuck
}
if (tmpy > MAXYPOS) {
// d = ball.getCoors().x < nxt.getCoors().x;
d = shootingLeft;
// which way should it be facing
upordown = nxt.getAngle() > 0 && nxt.getAngle() < Math.PI ? -1 : 1;
// by which side is it stuck
}
if (d) Main2.nxt.adjustWheelSpeeds(0, upordown * 600);
else Main2.nxt.adjustWheelSpeeds(upordown * 600, 0);
Tools.rest(600);
Main2.nxt.stop();
continue;
}
// target point is the shooting position
// returns ball coors if there is no shooting position
Point target = findPositionToShootFrom(ball.getCoors());
// if the other robot is less than 300 away, go for the ball
if (ball.getCoors().distance(opponentRobot.getCoors()) < 300) target = ball.getCoors();
// distance between the robot and the ball
int dist = (int) Tools.getDistanceBetweenPoint(nxt.getCoors(), ball.getCoors());
// if we are close to the ball AND it is not behind us, go for the
// ball
if (shootingLeft && (ball.getCoors().x - 30) < nxt.getCoors().x && dist < 70) {
target = ball.getCoors();
}
if (!shootingLeft && (ball.getCoors().x + 30) > nxt.getCoors().x && dist < 70) {
target = ball.getCoors();
}
// if we are close to the ball: slow down
// gear = dist < 100 ? (dist < 70 ? 4 : 7) : 12;
// if has ball and can shoot -> shoot
if (hasBall()) {
// if pointing to the other half -> shoot (also, don't kick
// 10cm~30px from the wall)
// 10cm = by how much does the kicker stick our when it kicks
if (isPointingTheOtherWay()
&& !(tmpy < MINYPOS + 30 && nxt.getAngle() > Math.PI && nxt.getAngle() < 2 * Math.PI)
&& !(tmpy > MAXYPOS - 30 && nxt.getAngle() > 0 && nxt.getAngle() < Math.PI)) {
nxt.kick();
nxt.adjustWheelSpeeds(600, 600);
Tools.rest(50);
continue;
}
System.out.println("hasBall");
if (isClearToShoot(ball.getCoors())) {
System.out.println("isClearToShoot");
nxt.adjustWheelSpeeds(600, 600);
nxt.kick();
Tools.rest(50);
continue;
} else {
// if has ball but cannot shoot: just go for goal
target = ball.getCoors(); // goalPoints[0];
}
}
// print the possible goals.
for (int i = 0; i < goalPoints.length; i++) {
printOnVisionFix(goalPoints[i], Color.red);
}
// mark blue possible shoothig position
printOnVisionFix(currentGoal, Color.blue);
// if close enough, rotate.
{
if (dist < 50 && !ball.getCoors().equals(target)) {
double a = getAngleToFacePoint(currentGoal);
if (Math.toDegrees(a) > 10) {
long l = System.currentTimeMillis();
if (l - lr > 1000) {
nxt.rotateBy(a);
lr = l;
}
Tools.rest(50);
continue;
}
}
}
// Get a path from the path planner, fiddle with it to get the correct waypoint, move, iterate
ArrayList<Point> path =
PathSearch.getPath(
new Point(ball.getCoors().x, 480 - ball.getCoors().y),
new Point(nxt.getCoors().x, 480 - nxt.getCoors().y),
(int) Math.toDegrees(nxt.getAngle()),
new Point(opponentRobot.getCoors().x, 480 - opponentRobot.getCoors().y));
for (int j = 0; j < path.size(); j++) {
printOnVision(path.get(j), Color.red);
}
if (path.size() > 2) target = path.remove(2);
else if (path.size() > 1) target = path.remove(1);
else target = path.remove(0);
target.y = 480 - target.y;
printOnVisionFix(target, Color.red);
moveTo(target);
Tools.rest(50);
}
}
