test fixes

Author: Andrewyx

src/proto/message_translation/tbots_protobuf.cpp

@@ -513,7 +513,7 @@ double convertMaxAllowedSpeedModeToMaxAllowedSpeed(
     switch (max_allowed_speed_mode)
     {
         case TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT:
-            return robot_constants.robot_max_speed_trajectory_m_per_s;
+            return robot_constants.robot_trajectory_max_speed_m_per_s;
         case TbotsProto::MaxAllowedSpeedMode::STOP_COMMAND:
             return STOP_COMMAND_ROBOT_MAX_SPEED_METERS_PER_SECOND -
                    STOP_COMMAND_SPEED_SAFETY_MARGIN_METERS_PER_SECOND;

src/shared/robot_constants.h

@@ -90,7 +90,7 @@ struct RobotConstants
 
     // The maximum speed that the trajectory planner is allowed to command the robot to
     // move at, while still leaving headroom for the PID to apply correction on lag. [m/s]
-    float robot_max_speed_trajectory_m_per_s;
+    float robot_trajectory_max_speed_m_per_s;
 
     // The maximum acceleration achievable by our robots [m/s^2]
     float robot_max_acceleration_m_per_s_2;
@@ -159,14 +159,14 @@ constexpr RobotConstants createRobotConstants()
 
         // Robot's linear movement constants
         .robot_max_speed_m_per_s            = 3.0f,
-        .robot_max_speed_trajectory_m_per_s = 2.5f,
-        .robot_max_acceleration_m_per_s_2   = 2.0f,
-        .robot_max_deceleration_m_per_s_2   = 1.5f,
+        .robot_trajectory_max_speed_m_per_s = 3.0f,
+        .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            = 6.0f,
-        .robot_max_ang_speed_trajectory_rad_per_s = 5.0f,
-        .robot_max_ang_acceleration_rad_per_s_2   = 10.0f,
+        .robot_max_ang_speed_rad_per_s            = 10.0f,
+        .robot_max_ang_speed_trajectory_rad_per_s = 7.0f,
+        .robot_max_ang_acceleration_rad_per_s_2   = 30.0f,
 
         .wheel_radius_meters = 0.03f,
 

src/software/ai/evaluation/intercept.cpp

@@ -97,7 +97,7 @@ Point findOvershootInterceptPosition(const Robot& robot, const Point intercept_p
     Point best_position   = intercept_position;
     double final_speed    = step_speed;
     bool finished         = false;
-    double max_speed      = robot.robotConstants().robot_max_speed_trajectory_m_per_s;
+    double max_speed      = robot.robotConstants().robot_trajectory_max_speed_m_per_s;
     double max_acc        = robot.robotConstants().robot_max_acceleration_m_per_s_2;
 
     while (!finished)

src/software/embedded/primitive_executor.cpp

@@ -69,6 +69,11 @@ void PrimitiveExecutor::updateState(const Point& position, const Vector& velocit
     orientation_      = orientation;
     angular_velocity_ = angular_velocity;
 
+    if (!current_primitive_.has_move())
+    {
+        return;
+    }
+
     // If we are lagging behind trajectory too much, we have stalled! We need to
     // regenerate trajectory.
     if (trajectory_path_.has_value())

src/software/embedded/thunderloop.cpp

@@ -291,20 +291,20 @@ inline void Thunderloop::processLocalizationUpdates()
             localToGlobalVelocity(createVector(status.local_velocity()),
                                   robot_localizer_.getOrientation()),
             createAngularVelocity(status.angular_velocity())});
+    }
 
-        Vector linear_acceleration;
-        if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value())
-        {
-            const auto accel    = imu_poll->linear_acceleration.value();
-            linear_acceleration = Vector(accel[0], accel[1]);
-        }
+    Vector linear_acceleration;
+    if (imu_poll.has_value() && imu_poll->linear_acceleration.has_value())
+    {
+        const auto accel    = imu_poll->linear_acceleration.value();
+        linear_acceleration = Vector(accel[0], accel[1]);
+    }
 
-        robot_localizer_.step(linear_acceleration);
+    robot_localizer_.step(linear_acceleration);
 
-        primitive_executor_.updateState(
-            robot_localizer_.getPosition(), robot_localizer_.getVelocity(),
-            robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity());
-    }
+    primitive_executor_.updateState(
+        robot_localizer_.getPosition(), robot_localizer_.getVelocity(),
+        robot_localizer_.getOrientation(), robot_localizer_.getAngularVelocity());
 }
 
 inline void Thunderloop::processPrimitiveExecution(

src/software/world/robot.cpp

@@ -219,7 +219,7 @@ Duration Robot::getTimeToPosition(const Point& destination,
     double final_velocity_1d   = final_velocity.dot(dist_vector.normalize());
 
     return getTimeToTravelDistance(dist,
-                                   robot_constants_.robot_max_speed_trajectory_m_per_s,
+                                   robot_constants_.robot_trajectory_max_speed_m_per_s,
                                    robot_constants_.robot_max_acceleration_m_per_s_2,
                                    initial_velocity_1d, final_velocity_1d);
 }