improve stop (#617)

roboteam_ai/src/skills/ActiveStop.cpp

@@ -23,49 +23,47 @@ void ActiveStop::onInitialize() {
 }
 
 Skill::Status ActiveStop::onUpdate() {
-    if (attacker)
+    if (attacker) {
         targetPos = getOffensiveActivePoint();
-    else
+    } else {
         targetPos = getDefensiveActivePoint();
+    }
+
+    if (robot->pos.dist(targetPos) > 0.3) {
+        command.w = static_cast<float>((targetPos - robot->pos).angle());
+    } else {
+        command.w = static_cast<float>((ball->pos - robot->pos).angle());
+    }
 
-    command.w = static_cast<float>((targetPos - robot->pos).angle());
     Vector2 velocity = robot->getNumtreePosControl()->getRobotCommand(robot, targetPos).vel;
     command.x_vel = static_cast<float>(velocity.x);
     command.y_vel = static_cast<float>(velocity.y);
     publishRobotCommand();
     return Status::Running;
-
-
 }
 
 void ActiveStop::onTerminate(Skill::Status s) {
     attack = -1;
 }
 
 Vector2 ActiveStop::getOffensiveActivePoint() {
-
     Vector2 penaltyPos = rtt::ai::world::field->getPenaltyPoint(false);
-    Vector2 ballPos = rtt::ai::world::world->getBall()->pos;
-
-    Vector2 offset = (penaltyPos - ballPos).stretchToLength(0.666); // ssl rule + some buffer + rtt spirit
-    if (rtt::ai::world::field->pointIsInDefenceArea(ballPos + offset)){
-        return ((ballPos+offset).rotate(M_PI).stretchToLength(2));
-    }
-    return ballPos + offset;
-
+    return getPoint(penaltyPos);
 }
 
 Vector2 ActiveStop::getDefensiveActivePoint() {
-
     Vector2 penaltyPos = rtt::ai::world::field->getPenaltyPoint(true);
-    Vector2 ballPos = rtt::ai::world::world->getBall()->pos;
+    return getPoint(penaltyPos);
+}
+
+Vector2 ActiveStop::getPoint(const Vector2 &penaltyPos) {
+    Vector2 ballPos = world::world->getBall()->pos;
 
-    Vector2 offset = (penaltyPos - ballPos).stretchToLength(0.666); // ssl rule + some buffer + rtt spirit
-    if (rtt::ai::world::field->pointIsInDefenceArea(ballPos + offset)){
-        return (((ballPos+offset).rotate(M_PI)).stretchToLength(2));
+    Vector2 offset = (penaltyPos - ballPos).stretchToLength(1.0); // ssl rule + significant buffer
+    if (world::field->pointIsInDefenceArea(ballPos + offset)) {
+        return (((ballPos + offset).rotate(M_PI)).stretchToLength(2));
     }
     return ballPos + offset;
-
 }
 
 }

roboteam_ai/src/skills/ActiveStop.h

@@ -24,6 +24,7 @@ class ActiveStop : public Skill {
         static Vector2 getOffensiveActivePoint();
         static Vector2 getDefensiveActivePoint();
 
+        static Vector2 getPoint(const Vector2 &penaltyPos);
 };
 }
 }