[Cont'd] Improved ball placement from RoboCup 2024

docs/fsm-diagrams.md

@@ -33,20 +33,24 @@ stateDiagram-v2
 classDef terminate fill:white,color:black,font-weight:bold
 direction LR
 [*] --> StartState
-StartState --> AlignPlacementState : [!shouldKickOffWall]\n<i>alignPlacement</i>
-StartState --> KickOffWallState : [shouldKickOffWall]
-KickOffWallState --> KickOffWallState : [!kickDone && shouldKickOffWall]\n<i>kickOffWall</i>
-KickOffWallState --> KickOffWallState : [kickDone]
-KickOffWallState --> AlignPlacementState : [!kickDone]
-AlignPlacementState --> KickOffWallState : [shouldKickOffWall]
+StartState --> AlignPlacementState : [!shouldPickOffWall]\n<i>alignPlacement</i>
+StartState --> AlignWallState : [shouldPickOffWall]\n<i>alignWall</i>
+AlignWallState --> AlignWallState : [!wallAlignDone && shouldPickOffWall]\n<i>alignWall</i>
+AlignWallState --> PickOffWallState : [wallAlignDone]\n<i>setPickOffDest, pickOffWall</i>
+AlignWallState --> AlignPlacementState : [!shouldPickOffWall]\n<i>alignPlacement</i>
+PickOffWallState --> PickOffWallState : [!wallPickOffDone]\n<i>pickOffWall</i>
+PickOffWallState --> ReleaseBallState : [wallPickOffDone]\n<i>startWait</i>
+AlignPlacementState --> AlignWallState : [shouldPickOffWall]\n<i>alignWall</i>
 AlignPlacementState --> AlignPlacementState : [!alignDone]\n<i>alignPlacement</i>
-AlignPlacementState --> PlaceBallState : [alignDone]
+AlignPlacementState --> PlaceBallState : [alignDone]\n<i>placeBall</i>
 PlaceBallState --> PlaceBallState : [!ballPlaced]\n<i>placeBall</i>
-PlaceBallState --> WaitState : [ballPlaced]\n<i>startWait</i>
-WaitState --> WaitState : [!waitDone]
-WaitState --> RetreatState : [waitDone]
+PlaceBallState --> ReleaseBallState : [ballPlaced]\n<i>startWait</i>
+ReleaseBallState --> ReleaseBallState : [!waitDone && ballPlaced]\n<i>releaseBall</i>
+ReleaseBallState --> StartState : [!ballPlaced]
+ReleaseBallState --> RetreatState : [waitDone]\n<i>retreat</i>
 RetreatState --> Terminate:::terminate : [retreatDone && ballPlaced]
 RetreatState --> RetreatState : [ballPlaced]\n<i>retreat</i>
+RetreatState --> StartState : [!ballPlaced]
 
 ```
 
@@ -403,7 +407,7 @@ direction LR
 MoveState --> MoveState : [!moveDone]\n<i>updateMove</i>
 MoveState --> Terminate:::terminate : [moveDone]\n<i>updateMove</i>
 Terminate:::terminate --> MoveState : [!moveDone]\n<i>updateMove</i>
-Terminate:::terminate --> Terminate:::terminate : <i>updateMove</i>
+Terminate:::terminate --> Terminate:::terminate : <i>SET_STOP_PRIMITIVE_ACTION</i>
 
 ```
 

src/cli/cli_params.py

@@ -88,4 +88,4 @@ class Platform(str, Enum):
 
 EnableThunderscopeOption = Annotated[bool, Option("-t", "--enable_thunderscope")]
 EnableVisualizerOption = Annotated[bool, Option("-v", "--enable_visualizer")]
-StopAIOnStartOption = Annotated[bool, Option("-t", "--stop_ai_on_start")]
+StopAIOnStartOption = Annotated[bool, Option("-s", "--stop_ai_on_start")]

src/proto/message_translation/tbots_protobuf.cpp

@@ -499,6 +499,8 @@ int convertDribblerModeToDribblerSpeed(TbotsProto::DribblerMode dribbler_mode,
             return robot_constants.max_force_dribbler_speed_rpm;
         case TbotsProto::DribblerMode::OFF:
             return 0;
+        case TbotsProto::DribblerMode::RELEASE_BALL_SLOW:
+            return robot_constants.release_ball_dribbler_speed_rpm;
         default:
             LOG(WARNING) << "DribblerMode is invalid" << std::endl;
             return 0;
@@ -518,6 +520,12 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed(
                    STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND;
         case TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED:
             return COLLISION_ALLOWED_ROBOT_MAX_SPEED_METERS_PER_SECOND;
+        case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_RETREAT:
+            return robot_constants.ball_placement_retreat_max_speed_m_per_s;
+        case TbotsProto::MaxAllowedSpeedMode::BALL_PLACEMENT_WALL_DRIBBLE:
+            return robot_constants.ball_placement_wall_max_speed_m_per_s;
+        case TbotsProto::MaxAllowedSpeedMode::DRIBBLE:
+            return robot_constants.dribble_speed_m_per_s;
         default:
             LOG(WARNING) << "MaxAllowedSpeedMode is invalid" << std::endl;
             return 0.0;

src/proto/parameters.proto

@@ -462,7 +462,7 @@ message SensorFusionConfig
 
     // Number of dropped detections before we consider the ball not to be in the dribbler
     required int32 num_dropped_detections_before_ball_not_in_dribbler = 12
-        [default = 3, (bounds).min_int_value = 0, (bounds).max_int_value = 1000];
+        [default = 10, (bounds).min_int_value = 0, (bounds).max_int_value = 1000];
 
     // Possession tracker for determining which team has possession of the ball
     required PossessionTrackerConfig possession_tracker_config = 13;
@@ -562,6 +562,13 @@ message RobotNavigationObstacleConfig
     // robot's position for obstacle avoidance
     required double dynamic_enemy_robot_obstacle_horizon_sec = 4
         [default = 0.4, (bounds).min_double_value = 0.0, (bounds).max_double_value = 2.0];
+
+    // Additional amount to inflate ball obstacle by for extra safety
+    required double additional_ball_obstacle_inflation_meters = 5 [
+        default                   = 0.05,
+        (bounds).min_double_value = 0.0,
+        (bounds).max_double_value = 0.5
+    ];
 }
 
 message CostVisualizationConfig

src/proto/primitive.proto

@@ -57,9 +57,10 @@ enum MotionConstraint
 
 enum DribblerMode
 {
-    OFF        = 0;
-    INDEFINITE = 1;
-    MAX_FORCE  = 2;
+    OFF               = 0;
+    INDEFINITE        = 1;
+    MAX_FORCE         = 2;
+    RELEASE_BALL_SLOW = 3;
 }
 
 enum MaxAllowedSpeedMode
@@ -70,6 +71,12 @@ enum MaxAllowedSpeedMode
     STOP_COMMAND = 1;
     // Move at speed at which collisions are allowed
     COLLISIONS_ALLOWED = 2;
+    // Slow speed for retreating in ball placement
+    BALL_PLACEMENT_RETREAT = 3;
+    // Slow speed for picking the ball off the wall in ball placement
+    BALL_PLACEMENT_WALL_DRIBBLE = 4;
+    // Slower speed for dribbling
+    DRIBBLE = 5;
 }
 
 message AutoChipOrKick

src/shared/2021_robot_constants.cpp

@@ -17,16 +17,20 @@ RobotConstants_t create2021RobotConstants(void)
         .front_of_robot_width_meters = 0.11f,
         .dribbler_width_meters       = 0.07825f,
         // Dribbler speeds are negative as that is the direction that sucks the ball in
-        .indefinite_dribbler_speed_rpm = -10000,
-        .max_force_dribbler_speed_rpm  = -12000,
+        .indefinite_dribbler_speed_rpm   = -10000,
+        .max_force_dribbler_speed_rpm    = -12000,
+        .release_ball_dribbler_speed_rpm = 2000,
 
         // Motor constant
         .motor_max_acceleration_m_per_s_2 = 4.5f,
 
         // Robot's linear movement constants
-        .robot_max_speed_m_per_s          = 3.000f,
-        .robot_max_acceleration_m_per_s_2 = 3.0f,
-        .robot_max_deceleration_m_per_s_2 = 3.0f,
+        .robot_max_speed_m_per_s                  = 3.000f,
+        .ball_placement_wall_max_speed_m_per_s    = 0.3f,
+        .ball_placement_retreat_max_speed_m_per_s = 0.3f,
+        .dribble_speed_m_per_s                    = 1.5f,
+        .robot_max_acceleration_m_per_s_2         = 3.0f,
+        .robot_max_deceleration_m_per_s_2         = 3.0f,
 
         // Robot's angular movement constants
         .robot_max_ang_speed_rad_per_s          = 10.0f,

src/shared/constants.h

@@ -61,25 +61,25 @@ static const short unsigned int MAXIMUM_TRANSFER_UNIT_BYTES = 1500;
 /* Game Rules */
 // The max allowed speed of the ball, in metres per second
 // https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_speed
-static const double BALL_MAX_SPEED_METERS_PER_SECOND = 6.5;
+static constexpr double BALL_MAX_SPEED_METERS_PER_SECOND = 6.5;
 // The safe max speed of the ball that we should shoot at, in metres per second
-static const double BALL_SAFE_MAX_SPEED_METERS_PER_SECOND =
+static constexpr double BALL_SAFE_MAX_SPEED_METERS_PER_SECOND =
     BALL_MAX_SPEED_METERS_PER_SECOND - 0.5;
 // The distance that the ball has to travel for it to be considered in play
 // after a kick-off, free kick, or penalty kick.
 // https://robocup-ssl.github.io/ssl-rules/sslrules.html#_ball_in_and_out_of_play
-static const double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05;
+static constexpr double BALL_IN_PLAY_DISTANCE_THRESHOLD_METERS = 0.05;
 // The max allowed height of the robots, in metres
-static const double ROBOT_MAX_HEIGHT_METERS = 0.15;
+static constexpr double ROBOT_MAX_HEIGHT_METERS = 0.15;
 // The max allowed radius of the robots, in metres
-static const double ROBOT_MAX_RADIUS_METERS = 0.09;
+static constexpr double ROBOT_MAX_RADIUS_METERS = 0.09;
 // The distance from the center of the robot to the front face (the flat part), in meters
-static const double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078;
+static constexpr double DIST_TO_FRONT_OF_ROBOT_METERS = 0.078;
 // The approximate radius of the ball according to the SSL rulebook
-static const double BALL_MAX_RADIUS_METERS = 0.0215;
+static constexpr double BALL_MAX_RADIUS_METERS = 0.0215;
 // According to the rules, 80% of the ball must be seen at all times. Robots may not
 // cover more than 20% of the ball
-static const double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2;
+static constexpr double MAX_FRACTION_OF_BALL_COVERED_BY_ROBOT = 0.2;
 
 // The radius of a circle region where ball placement is acceptable (in meters).
 constexpr double BALL_PLACEMENT_TOLERANCE_RADIUS_METERS = 0.15;

src/shared/robot_constants.h

@@ -73,12 +73,25 @@ typedef struct RobotConstants
     // the ball [rpm]
     int max_force_dribbler_speed_rpm;
 
+    // Speed at which to spin the dribbler to release the ball right in front of a
+    // stationary robot [rpm]
+    int release_ball_dribbler_speed_rpm;
+
     // The maximum acceleration achievable by our motors [m/s^2]
     float motor_max_acceleration_m_per_s_2;
 
     // The maximum speed achievable by our robots, in metres per second [m/s]
     float robot_max_speed_m_per_s;
 
+    // The max speed at which we will pick the ball off the wall
+    float ball_placement_wall_max_speed_m_per_s;
+
+    // The max speed at which we will retreat away from the ball after placing it [m/s]
+    float ball_placement_retreat_max_speed_m_per_s;
+
+    // The max speed at which we dribble the ball
+    float dribble_speed_m_per_s;
+
     // The maximum acceleration achievable by our robots [m/s^2]
     float robot_max_acceleration_m_per_s_2;