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); } }