Feature/dont look at ball in ball handle (#621)

* Take everything useful from revealPresentation branch

* Fix build problem

* Fix build problem

* Fix 2 comments

* Update ShotController.cpp

* Last day design lab

* Merge branch 'merge-reveal-presentation' of https://github.com/RoboTeamTwente/roboteam_ai into merge-reveal-presentation

# Conflicts:
#	roboteam_ai/src/control/shotControllers/ShotController.cpp

* __Commit1__ 123334

* drive towards line in ballhandle

* drive towards line v2

* Working BallPlacement (tm)

* fix robot going outside field

* thijs is gay

* fix robot going outside field v2

* fix out of field velocity v2

* 1

* fix real life

* 2

* fix two tests

* fix ambiguity

* fix

* Grsim updates while keeping functionality for real robots the same

* add OVERWRITE_GRSIM() to constants, fix tests

* Add comment to OVERWRITE_GRSIM( )

roboteam_ai/src/coach/PassCoach.cpp

@@ -131,7 +131,7 @@ void PassCoach::updatePassProgression() {
     }
 
 }
-bool PassCoach::validReceiver(RobotPtr passer, RobotPtr receiver) {
+bool PassCoach::validReceiver(const RobotPtr& passer, const RobotPtr& receiver) {
     auto ball = world::world->getBall();
 
     if (!ball || !passer || !receiver) {
@@ -146,7 +146,7 @@ bool PassCoach::validReceiver(RobotPtr passer, RobotPtr receiver) {
     if((passer->pos - receiver->pos).length() < MIN_PASS_DISTANCE) {
         return false;
     }
-    return receiver->pos.x - ball->pos.x >= 0;
+    return true;
 }
 
 } // coach

roboteam_ai/src/coach/PassCoach.h

@@ -32,7 +32,7 @@ class PassCoach {
     virtual int determineReceiver(int passerID);
     bool passTakesTooLong();
     void updatePassProgression();
-    bool validReceiver(RobotPtr passer, RobotPtr receiver);
+    bool validReceiver(const RobotPtr& passer, const RobotPtr& receiver);
 
 private:
 

roboteam_ai/src/conditions/ShouldHandleBall.cpp

@@ -15,12 +15,16 @@ ShouldHandleBall::ShouldHandleBall(std::string name, bt::Blackboard::Ptr blackbo
 std::string ShouldHandleBall::node_name() { return "ShouldHandleBall"; }
 
 ShouldHandleBall::Status ShouldHandleBall::onUpdate() {
-    bool passExists = coach::g_pass.getRobotBeingPassedTo() != - 1 && ! coach::g_pass.isPassed();
-    if (passExists && coach::g_pass.getRobotPassing() == robot->id) {
-        return Status::Success;
+    bool passExists = coach::g_pass.getRobotBeingPassedTo() != -1;
+    if (passExists) {
+        if (!coach::g_pass.isPassed() && coach::g_pass.getRobotPassing() == robot->id) {
+            return Status::Success;
+        } else {
+            return Status::Failure;
+        }
     }
 
-    if (! passExists && coach::getBallCoach->getBallGetterID() == robot->id) {
+    if (!passExists && coach::getBallCoach->getBallGetterID() == robot->id) {
         return Status::Success;
     }
 

roboteam_ai/src/conditions/TwoRobotBallPlacement.cpp

@@ -13,20 +13,18 @@ TwoRobotBallPlacement::TwoRobotBallPlacement(std::string name, bt::Blackboard::P
         :Condition(std::move(name), std::move(blackboard)) {};
 
 bt::Node::Status TwoRobotBallPlacement::onUpdate() {
-    int robotClosestToBallID = world::world->getRobotClosestToBall(OUR_ROBOTS)->id;
-
-    Vector2 ballPlacementPosition = coach::g_ballPlacement.getBallPlacementPos();
-    int robotClosestToBallPlacementPosition = world::world->getRobotClosestToPoint(ballPlacementPosition, OUR_ROBOTS)->id;
-
-    bool robotClosestToBallIsClosestToTarget = robotClosestToBallID == robotClosestToBallPlacementPosition;
-    bool distanceFromBallToTargetIsSmall =  (ballPlacementPosition - ball->pos).length() < MAX_ONE_ROBOT_BALLPLACEMENT_DIST_TO_TARGET;
-
-
-    if (!robotClosestToBallIsClosestToTarget && !distanceFromBallToTargetIsSmall) {
+    Vector2 ballPlacementPos = coach::g_ballPlacement.getBallPlacementPos();
+    Vector2 ballPos = ball->pos;
+    auto us = world::world->getUs();
+    if (us.size() < 2) {
+        return Status::Failure;
+    }
+
+    if ((ball->pos - ballPlacementPos).length() < 2.1) {
+        return Status::Failure;
+    } else {
         return Status::Success;
-    } 
-    return Status::Failure;
-
+    }
 }
 
 }

roboteam_ai/src/control/ballHandling/BallHandlePosControl.cpp

@@ -21,12 +21,13 @@ namespace control {
 BallHandlePosControl::BallHandlePosControl(bool canMoveInDefenseArea) {
 
     dribbleForwards = new DribbleForwards(ERROR_MARGIN, ANGLE_ERROR_MARGIN, ballPlacementAccuracy, maxForwardsVelocity);
-    dribbleBackwards = new DribbleBackwards(ERROR_MARGIN, ANGLE_ERROR_MARGIN, ballPlacementAccuracy, maxBackwardsVelocity);
+    dribbleBackwards = new DribbleBackwards(ERROR_MARGIN, ANGLE_ERROR_MARGIN, ballPlacementAccuracy,
+            maxBackwardsVelocity);
     rotateWithBall = new RotateWithBall();
     rotateAroundBall = new RotateAroundBall();
 
     setCanMoveInDefenseArea(canMoveInDefenseArea);
-    setAvoidBallDistance(TARGET_BALL_DISTANCE*0.95);
+    setAvoidBallDistance(MAX_BALL_DISTANCE * 0.92);
 }
 
 RobotCommand BallHandlePosControl::getRobotCommand(const RobotPtr &r, const Vector2 &targetP) {
@@ -59,6 +60,11 @@ RobotCommand BallHandlePosControl::getRobotCommand(const RobotPtr &r, const Vect
     double expectedDelay = 0.04;
     ball = std::make_shared<world::Ball>(world::Ball(*world::world->getBall()));
     robot = world::world->getFutureRobot(r, expectedDelay);
+
+    if ((targetPos - targetP).length2() > 0.10) {
+        dribbleBackwards->reset();
+        dribbleForwards->reset();
+    }
     targetPos = targetP;
     targetAngle = targetA;
 
@@ -91,8 +97,10 @@ RobotCommand BallHandlePosControl::getRobotCommand(const RobotPtr &r, const Vect
                     dribbleForwards->getForwardsProgression() != DribbleForwards::ForwardsProgress::START);
 
     bool shouldGetBall = alreadyDribbling ?
-            (ballIsMovingTooFast && ! robotIsTouchingBall) || (robotDoesNotHaveBall && robotIsTooFarFromBall) :
-            (ballIsMovingTooFast && ! robotIsTouchingBall) || (robotDoesNotHaveBall || robotIsTooFarFromBall);
+                         (ballIsMovingTooFast && ! robotIsTouchingBall)
+                                 || (robotDoesNotHaveBall && robotIsTooFarFromBall) :
+                         (ballIsMovingTooFast && ! robotIsTouchingBall)
+                                 || (robotDoesNotHaveBall || robotIsTooFarFromBall);
 
     bool ballIsOutsideField = ! world::field->pointIsInField(ball->pos, 0.0);
     if (ballIsOutsideField) {
@@ -267,8 +275,6 @@ RobotCommand BallHandlePosControl::goToBall(const Vector2 &targetBallPos, Travel
 }
 
 RobotCommand BallHandlePosControl::goToMovingBall() {
-    Vector2 numTreesTarget;
-
     Vector2 ballStillPosition = ball->getBallStillPosition();
 
     LineSegment ballLine = LineSegment(ball->pos, ball->pos + ball->vel);
@@ -284,29 +290,52 @@ RobotCommand BallHandlePosControl::goToMovingBall() {
             ballToProjectionDistance/ball->vel.length();
 
     if (robotIsBehindBall && robotCanInterceptBall) {
-        numTreesTarget = projectionPosition;
-        auto robotCommand = NumTreePosControl::getRobotCommand(robot, numTreesTarget);
+        return interceptMovingBall(projectionPosition, ballToProjectionDistance, robotAngleTowardsBallVel);
+    }
+    return goBehindBall(ballStillPosition);
+}
 
-        robotCommand.angle = (ball->pos - robot->pos).toAngle();
-        if (fabs(robotAngleTowardsBallVel) > M_PI*0.05) {
-            robotCommand.vel += ball->vel.stretchToLength(
-                    fabs((robot->pos - ball->pos).toAngle() - ball->vel.toAngle()));
+RobotCommand BallHandlePosControl::goBehindBall(const Vector2 &ballStillPosition) {
+    Vector2 numTreesTarget = ballStillPosition;
+    if (! world::field->pointIsInField(ballStillPosition, Constants::ROBOT_RADIUS())) {
+        LineSegment ballLine = LineSegment(ball->pos, ballStillPosition);
+        Polygon fieldEdge = world::field->getFieldEdge(Constants::ROBOT_RADIUS());
+
+        auto intersections = fieldEdge.intersections(ballLine);
+        if (intersections.size() == 1) {
+            numTreesTarget = intersections[0];
         }
-        else if (ballToProjectionDistance/ball->vel.length() > 0.8) {
-            robotCommand.vel -= ball->vel;
+        else {
+            numTreesTarget = ControlUtils::projectPositionToWithinField(ballStillPosition);
         }
-
-        return robotCommand;
     }
 
-    numTreesTarget = ControlUtils::projectPositionToWithinField(ballStillPosition);
     auto robotCommand = NumTreePosControl::getRobotCommand(robot, numTreesTarget);
 
-    if (NumTreePosControl::getCurrentCollisionWithRobot().getCollisionType() == Collision::CollisionType::BALL) {
+    if (getCurrentCollisionWithRobot().getCollisionType() == Collision::BALL) {
         robotCommand.vel = (ball->pos - robot->pos).stretchToLength(ball->vel.length());
     }
 
-    robotCommand.vel += ball->vel;
+    if (world::field->pointIsInField(robot->pos + robot->vel)) {
+        robotCommand.vel += ball->vel;
+    }
+    return robotCommand;
+}
+
+RobotCommand BallHandlePosControl::interceptMovingBall(const Vector2 &projectionPosition,
+        double ballToProjectionDistance, const Angle &robotAngleTowardsBallVel) {
+    Vector2 numTreesTarget = projectionPosition;
+    auto robotCommand = NumTreePosControl::getRobotCommand(robot, numTreesTarget);
+
+    robotCommand.angle = (ball->pos - robot->pos).toAngle();
+    if (fabs(robotAngleTowardsBallVel) > M_PI*0.05) {
+        robotCommand.vel += ball->vel.stretchToLength(
+                fabs((robot->pos - ball->pos).toAngle() - ball->vel.toAngle()));
+    }
+    else if (ballToProjectionDistance/ball->vel.length() > 0.8) {
+        robotCommand.vel -= ball->vel;
+    }
+
     return robotCommand;
 }
 
@@ -338,14 +367,23 @@ RobotCommand BallHandlePosControl::controlWithPID(PID &xpid, PID &ypid, const Ro
     RobotCommand pidCommand = robotCommand;
     pidCommand.vel.x = xpid.getOutput(robot->vel.x, robotCommand.vel.x);
     pidCommand.vel.y = ypid.getOutput(robot->vel.y, robotCommand.vel.y);
+    double minVel = 0.112;
+    if (pidCommand.vel.length() < minVel) {
+        pidCommand.vel = pidCommand.vel.stretchToLength(
+                std::max(minVel, pidCommand.vel.length() + ++ ticksNotMoving*0.006789));
+    }
+    else {
+
+        ticksNotMoving = 0;
+    }
     return pidCommand;
 }
 
 void BallHandlePosControl::updatePID(pidVals newPID) {
     if (newPID != lastPid) {
         lastPid = newPID;
-        xBallHandlePID.setPID(newPID);
-        yBallHandlePID.setPID(newPID);
+        xBallHandlePID.setPID(newPID, 1.0);
+        yBallHandlePID.setPID(newPID, 1.0);
     }
 }
 

roboteam_ai/src/control/ballHandling/BallHandlePosControl.h

@@ -28,14 +28,14 @@ class BallHandlePosControl : public NumTreePosControl {
         RotateWithBall* rotateWithBall;
         RotateAroundBall* rotateAroundBall;
 
-        double maxForwardsVelocity = Constants::GRSIM() ? 0.6 : 0.5;
-        double maxBackwardsVelocity = Constants::GRSIM() ? 0.3 : 0.7;
-        double ballPlacementAccuracy = 0.07;
+        double maxForwardsVelocity = Constants::GRSIM() ? 0.6 : 0.3;
+        double maxBackwardsVelocity = Constants::GRSIM() ? 0.4 : 0.3;
+        double ballPlacementAccuracy = 0.15;
 
         constexpr static double ERROR_MARGIN = 0.02;
-        constexpr static double ANGLE_ERROR_MARGIN = 0.02;
+        constexpr static double ANGLE_ERROR_MARGIN = 0.010*M_PI;
         constexpr static double MAX_BALL_DISTANCE = Constants::ROBOT_RADIUS()*2.0;
-        constexpr static double MIN_VEL_FOR_MOVING_BALL = 0.16;
+        constexpr static double MIN_VEL_FOR_MOVING_BALL = 0.3162277660168;
         constexpr static double TARGET_BALL_DISTANCE = Constants::ROBOT_RADIUS() + Constants::BALL_RADIUS();
         constexpr static double ROBOT_IS_TOUCHING_BALL = TARGET_BALL_DISTANCE*1.05;
 
@@ -44,6 +44,7 @@ class BallHandlePosControl : public NumTreePosControl {
         Vector2 targetPos;
         Angle targetAngle;
         Angle lockedAngle = 0;
+        int ticksNotMoving = 0;
 
         pidfVals pidfGoToBall = std::make_tuple(0.0, 0.0, 0.0, 1.0);
         PID xGoToBallPID = PID(pidfGoToBall);
@@ -99,6 +100,9 @@ class BallHandlePosControl : public NumTreePosControl {
         RobotCommand goToIdleBall(const Vector2 &targetBallPos, TravelStrategy travelStrategy,
                 bool ballIsFarFromTarget);
         RobotCommand finalizeBallHandle();
+        RobotCommand interceptMovingBall(const Vector2 &projectionPosition, double ballToProjectionDistance,
+                const Angle &robotAngleTowardsBallVel);
+        RobotCommand goBehindBall(const Vector2 &ballStillPosition);
 };
 
 } //control

roboteam_ai/src/control/ballHandling/DribbleBackwards.cpp

@@ -3,6 +3,8 @@
 // Created by thijs on 25-5-19.
 //
 
+#include <roboteam_ai/src/world/Field.h>
+#include <roboteam_ai/src/interface/api/Input.h>
 #include "../../control/ControlUtils.h"
 #include "DribbleBackwards.h"
 #include "RotateAroundBall.h"
@@ -36,14 +38,14 @@ void DribbleBackwards::updateBackwardsProgress() {
         printBackwardsProgress();
     }
 
-    if (backwardsProgress != DRIBBLING) waitingTicks = 25;
+    if (backwardsProgress != DRIBBLING && backwardsProgress != OVERSHOOTING) waitingTicks = 100;
 
     // check if we still have ball
     if (backwardsProgress != OVERSHOOTING &&
             backwardsProgress != DRIBBLING &&
             backwardsProgress != DRIBBLE_BACKWARDS) {
 
-        approachPosition = ball->pos + (robot->pos - ball->pos).stretchToLength(0.05);
+        approachPosition = ball->pos + (robot->pos - ball->pos).stretchToLength(-0.05);
     }
     if ((ball->pos - robot->pos).length2() > 0.5) {
         backwardsProgress = START;
@@ -83,14 +85,19 @@ void DribbleBackwards::updateBackwardsProgress() {
             backwardsProgress = DRIBBLING;
             return;
         }
+        if (-- waitingTicks < 0) {
+            failedOnce = true;
+            backwardsProgress = DRIBBLING;
+            return;
+        }
         return;
     }
     case DRIBBLING: {
         if (! robot->hasBall()) {
             backwardsProgress = APPROACHING;
             return;
         }
-        if (-- waitingTicks < 0) {
+        if (-- waitingTicks < 75) {
             backwardsDribbleLine = {robot->pos, finalTargetPos};
             backwardsProgress = DRIBBLE_BACKWARDS;
             return;
@@ -103,23 +110,16 @@ void DribbleBackwards::updateBackwardsProgress() {
             backwardsProgress = APPROACHING;
             return;
         }
-        Vector2 finalTargetToRobot = robot->pos - finalTargetPos;
-        Angle offsetAngle = finalTargetToRobot.toAngle() - robot->angle;
-        double maxOffsetAngle = M_PI*0.05;
-        if (fabs(offsetAngle) > maxOffsetAngle) {
-            if (finalTargetToRobot.length2() > Constants::ROBOT_RADIUS()*1.2) {
-                backwardsProgress = START;
-                return;
-            }
-        }
         if ((ball->pos - finalTargetPos).length2() < ballPlacementAccuracy*ballPlacementAccuracy) {
             backwardsProgress = SUCCESS;
             return;
         }
         return;
     }
     case FAIL:
-    case START:
+    case START: {
+        failedOnce = false;
+    }
     default:return;
     case SUCCESS: {
         if ((ball->pos - finalTargetPos).length2() < ballPlacementAccuracy*ballPlacementAccuracy) {
@@ -166,40 +166,58 @@ RobotCommand DribbleBackwards::sendTurnCommand() {
 RobotCommand DribbleBackwards::sendApproachCommand() {
     RobotCommand command;
     command.dribbler = 28;
-    command.vel = (ball->pos - robot->pos).stretchToLength(maxVel);
+    command.vel = (ball->pos - robot->pos).stretchToLength(std::min(0.2, maxVel));
     command.angle = lockedAngle;
     return command;
 }
 
 RobotCommand DribbleBackwards::sendOvershootCommand() {
     RobotCommand command;
-    command.dribbler = 28;
-    command.vel = (approachPosition - robot->pos).stretchToLength(maxVel);
+    command.dribbler = 30;
+    command.vel = (approachPosition - robot->pos).stretchToLength(std::min(0.2, maxVel));
     command.angle = lockedAngle;
+
+    if (failedOnce && !world::field->pointIsInField(ball->pos, Constants::ROBOT_RADIUS())) {
+        command.kickerForced = true;
+        command.kickerVel = 2;
+        command.kicker = true;
+    }
     return command;
 }
 
 RobotCommand DribbleBackwards::sendDribblingCommand() {
     RobotCommand command;
-    command.dribbler = 28;
+    command.dribbler = 31;
     command.angle = lockedAngle;
     return command;
 }
 
 RobotCommand DribbleBackwards::sendDribbleBackwardsCommand() {
     RobotCommand command;
-    command.dribbler = 28;
+    command.dribbler = 31;
     command.angle = lockedAngle;
     command.vel = lockedAngle.toVector2(- maxVel);
 
+
     // check if the robot is still on the virtual line from ball->pos to the target
     if (control::ControlUtils::distanceToLine(robot->pos,
-            backwardsDribbleLine.first, backwardsDribbleLine.second) > errorMargin*2.5) {
+            backwardsDribbleLine.first, backwardsDribbleLine.second) > errorMargin*5) {
         backwardsProgress = TURNING;
     }
 
+    Angle robotAngleTowardsLine = (finalTargetPos - robot->pos).toAngle() -
+            (backwardsDribbleLine.second - backwardsDribbleLine.first).toAngle();
+
+    Vector2 compensation = (robot->angle + M_PI_2).toVector2(std::min(robotAngleTowardsLine*2.72, 0.05));
+    command.vel += compensation;
+
+    interface::Input::drawData(interface::Visual::BALL_HANDLING, {compensation+robot->pos, robot->pos},
+            Qt::white, robot->id, interface::Drawing::ARROWS);
+    interface::Input::drawData(interface::Visual::BALL_HANDLING, {backwardsDribbleLine.first, backwardsDribbleLine.second},
+            Qt::white, robot->id, interface::Drawing::LINES_CONNECTED);
+
     // check if the ball is not too far right or too far left of the robot, and try to compensate for that
-    if (ball->visible) {
+    if (ball->visible && false) {
         Angle ballAngleRelativeToRobot = (ball->pos - robot->pos).toAngle() - robot->angle;
         command.vel += (robot->angle + M_PI_2).toVector2(ballAngleRelativeToRobot);
     }