fixed/improved angular orbit

include/roboteam_ai/stp/skills/OrbitAngular.h

@@ -22,6 +22,11 @@ class OrbitAngular : public Skill {
      * @return The name of this skill
      */
     const char* getName() override;
+
+    /**
+     * Counter for how many ticks the robot is within the error margin
+     */
+    int counter = 0;
 };
 }  // namespace rtt::ai::stp::skill
 

src/control/ControlUtils.cpp

@@ -86,7 +86,7 @@ double ControlUtils::determineKickForce(const double distance, stp::ShotType sho
 
     double kickForce;
     if (shotType == stp::ShotType::PASS) {
-        kickForce = 1.5;
+        kickForce = 1.8;
     } else if (shotType == stp::ShotType::TARGET) {
         kickForce = 0.5;
     } else {

src/stp/skills/Kick.cpp

@@ -6,12 +6,14 @@
 
 #include "stp/constants/ControlConstants.h"
 
+#include "roboteam_utils/Print.h"
+
 namespace rtt::ai::stp::skill {
 
 Status Kick::onUpdate(const StpInfo &info) noexcept {
     // Clamp and set kick velocity
     float kickVelocity = std::clamp(info.getKickChipVelocity(), control_constants::MIN_KICK_POWER, control_constants::MAX_KICK_POWER);
-
+    RTT_DEBUG(kickVelocity)
     // Set kick command
     command.kickType = KickType::KICK;
     command.kickSpeed = kickVelocity;

src/stp/skills/OrbitAngular.cpp

@@ -5,62 +5,49 @@
 #include "stp/skills/OrbitAngular.h"
 
 #include "stp/constants/ControlConstants.h"
+#include "roboteam_utils/Print.h"
 
 namespace rtt::ai::stp::skill {
 
 Status OrbitAngular::onUpdate(const StpInfo &info) noexcept {
-    double rpm = 20;  // Number of orbits (rotations) per minute
-    double circumference = (control_constants::ROBOT_RADIUS + stp::control_constants::BALL_RADIUS) * 2 * M_PI;
-
-    // Constants used to tweak the orbit to be a nice circle
-    constexpr double ANGLE_RATE_FACTOR = 1.0;
-    constexpr double VELOCITY_FACTOR = 1.2;
-
-    // Calculate angVel and rotational velocity to make a circular movement around the ball
-    double angVel = -rpm / 60 * 2 * M_PI * ANGLE_RATE_FACTOR;
-    double rotVel = rpm / 60 * circumference * VELOCITY_FACTOR;
-
-    Vector2 directionVector = info.getRobot()->get()->getAngle().toVector2();
-    Angle targetAngle = (info.getPositionToShootAt().value() - info.getRobot()->get()->getPos()).toAngle();
-
-    auto direction = Angle(directionVector).rotateDirection(targetAngle) ? -1.0 : 1.0;
-    // Since there is a small delay between sending kick and the ball being kicked, we want to adjust the targetAngle so that we kick slightly earlier
-    targetAngle = targetAngle.getValue() - angVel / 20.0 * direction;
-    direction = Angle(directionVector).rotateDirection(targetAngle) ? -1.0 : 1.0;
-
-    Vector2 normalVector = directionVector.rotate(direction * (M_PI_2 - (1.0 / 3.0) * M_PI));
-
-    // Let robot move in direction normal to the direction of the robot (sideways)
+    // initialization of local variables
+    Vector2 directionVector =  info.getRobot()->get()->getAngle().toVector2();  // Vector in direction robot is currently facing
+    Angle targetAngle = (info.getPositionToShootAt().value() - info.getRobot()->get()->getPos()).toAngle(); // Angle we want to have
+    auto direction = Angle(directionVector).rotateDirection(targetAngle) ? 1.0 : -1.0;  // Direction robot should rotate to
+    double speed = 0.1 + 4 * directionVector.toAngle().shortestAngleDiff(targetAngle); // Speed at which the robot should orbit. I made it relative to the angle so that we don't overshoot
+    Vector2 normalVector = directionVector.rotate(-direction * M_PI_2); // Direction robot should move to
+
+    // velocity vector the robot should follow
     Vector2 targetVelocity;
-    targetVelocity.x = normalVector.x;
-    targetVelocity.y = normalVector.y;
-
-    command.velocity = targetVelocity.stretchToLength(rotVel);
-
-    command.useAngularVelocity = true;
-    command.targetAngularVelocity = angVel * direction;
-    // RTT_DEBUG("Orbit set angular velocity: ", command.targetAngularVelocity);
+    targetVelocity.x = speed * normalVector.x;
+    targetVelocity.y = speed * normalVector.y;
 
-    // set command ID
+    // make robot commnad
     command.id = info.getRobot().value()->getId();
+    command.velocity = targetVelocity;
+    command.useAngularVelocity = true;
+    command.targetAngularVelocity = direction * targetVelocity.length(); // Scale the angular velocity to the absolute velocity for a nice circle
+    command.dribblerSpeed = stp::control_constants::MAX_DRIBBLER_CMD;
 
-    // set angle to kick at & turn on kickAtAngle
-    command.targetAngle = targetAngle;
-    command.kickAtAngle = true;
-    command.kickSpeed = std::clamp(info.getKickChipVelocity(), control_constants::MIN_KICK_POWER, control_constants::MAX_KICK_POWER);
-
-    int targetDribblerPercentage = std::clamp(info.getDribblerSpeed(), 0, 100);
-    double targetDribblerSpeed = targetDribblerPercentage / 100.0 * stp::control_constants::MAX_DRIBBLER_CMD;
-
-    command.dribblerSpeed = targetDribblerSpeed;
-
-    // forward the generated command to the ControlModule, for checking and limiting
+    // Send robot commands
     forwardRobotCommand(info.getCurrentWorld());
 
-    // Check if successful- Done after the ball has been kicked (if the velocity is low, we probably did not kick it, so we didn't finish successfully)
-    if (!info.getRobot()->get()->hasBall() && info.getBall()->get()->velocity.length() > control_constants::BALL_IS_MOVING_SLOW_LIMIT) {
+    // Check if successful
+    double errorMargin = stp::control_constants::GO_TO_POS_ANGLE_ERROR_MARGIN * M_PI_2 * 0.2; // Can be finetuned. smaller margin means preciser shooting but more prone to overshooting angle
+    if (directionVector.toAngle().shortestAngleDiff(targetAngle) < errorMargin) {
+        counter++;
+    }
+    else {
+        counter = 0;
+    }
+
+    // If the robot is within the error margin for 5 consecutive ticks, return success
+    if (counter > 2) {
+        command.dribblerSpeed = 0;
+        forwardRobotCommand(info.getCurrentWorld());
         return Status::Success;
-    } else {
+    }
+    else {
         return Status::Running;
     }
 }

src/stp/tactics/active/OrbitKick.cpp

@@ -12,7 +12,7 @@ namespace rtt::ai::stp::tactic {
 
 OrbitKick::OrbitKick() {
     // Create state machine of skills and initialize first skill
-    skills = rtt::collections::state_machine<Skill, Status, StpInfo>{skill::OrbitAngular()};
+    skills = rtt::collections::state_machine<Skill, Status, StpInfo>{skill::OrbitAngular(), skill::Kick()};
 }
 
 std::optional<StpInfo> OrbitKick::calculateInfoForSkill(StpInfo const &info) noexcept {