diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp index 86dd75f7d8..8961861eff 100644 --- a/src/software/embedded/primitive_executor.cpp +++ b/src/software/embedded/primitive_executor.cpp @@ -11,8 +11,8 @@ #include "software/physics/velocity_conversion_util.h" PrimitiveExecutor::PrimitiveExecutor( - const Duration time_step, const robot_constants::RobotConstants& robot_constants) - : current_primitive_(), robot_constants_(robot_constants), time_step_(time_step) + const robot_constants::RobotConstants& robot_constants) + : current_primitive_(), robot_constants_(robot_constants) { } @@ -78,9 +78,9 @@ AngularVelocity PrimitiveExecutor::getTargetAngularVelocity() std::unique_ptr PrimitiveExecutor::stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status) + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time) { - time_since_trajectory_creation_ += time_step_; + time_since_trajectory_creation_ += delta_time; status.set_running_primitive(true); switch (current_primitive_.primitive_case()) diff --git a/src/software/embedded/primitive_executor.h b/src/software/embedded/primitive_executor.h index 52d9de711e..f0fb3d2253 100644 --- a/src/software/embedded/primitive_executor.h +++ b/src/software/embedded/primitive_executor.h @@ -14,12 +14,10 @@ class PrimitiveExecutor public: /** * Constructor - * @param time_step Time step which this primitive executor operates in * @param robot_constants The robot constants for the robot which uses this primitive * executor */ - explicit PrimitiveExecutor(const Duration time_step, - const robot_constants::RobotConstants& robot_constants); + explicit PrimitiveExecutor(const robot_constants::RobotConstants& robot_constants); /** * Update primitive executor with a new Primitive @@ -39,13 +37,15 @@ class PrimitiveExecutor /** * Steps the current primitive and returns a direct control primitive with the * target wheel velocities + * * @param status The status of the primitive executor, set to false if current * primitive is a Stop primitive + * @param delta_time The elapsed time since the last primitive step * * @returns DirectControlPrimitive The direct control primitive msg */ std::unique_ptr stepPrimitive( - TbotsProto::PrimitiveExecutorStatus& status); + TbotsProto::PrimitiveExecutorStatus& status, Duration delta_time); private: /* @@ -70,10 +70,6 @@ class PrimitiveExecutor std::optional trajectory_path_; std::optional angular_trajectory_; - // TODO (#2855): Add dynamic time_step to `stepPrimitive` and remove this constant - // time step to be used, in Seconds - Duration time_step_; - // Estimated delay between a vision frame to AI processing to robot executing static constexpr double VISION_TO_ROBOT_DELAY_S = 0.03; diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp index c2d9c08a70..bcd527699c 100644 --- a/src/software/embedded/thunderloop.cpp +++ b/src/software/embedded/thunderloop.cpp @@ -84,7 +84,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants, kick_constant_(std::stoi(toml_config_client_->get(ROBOT_KICK_CONSTANT_CONFIG_KEY))), chip_pulse_width_( std::stoi(toml_config_client_->get(ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY))), - primitive_executor_(Duration::fromSeconds(1.0 / loop_hz), robot_constants) + primitive_executor_(robot_constants) { waitForNetworkUp(); @@ -286,8 +286,8 @@ void Thunderloop::runLoop() primitive_executor_.updatePrimitive(*createStopPrimitiveProto()); } - direct_control_ = - *primitive_executor_.stepPrimitive(primitive_executor_status_); + direct_control_ = *primitive_executor_.stepPrimitive( + primitive_executor_status_, Duration::fromSeconds(1.0 / loop_hz_)); } thunderloop_status_.set_primitive_executor_step_time_ms( diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index c609bff582..ba038290f0 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -20,10 +20,10 @@ ErForceSimulator::ErForceSimulator(const TbotsProto::FieldType& field_type, const robot_constants::RobotConstants& robot_constants, std::unique_ptr& realism_config, const bool ramping, - double primitive_executor_time_step) + Duration primitive_executor_time_step) : yellow_team_world_msg(std::make_unique()), blue_team_world_msg(std::make_unique()), - primitive_executor_time_step_s(primitive_executor_time_step), + primitive_executor_time_step(primitive_executor_time_step), frame_number(0), euclidean_to_four_wheel(robot_constants), robot_constants(robot_constants), @@ -280,14 +280,14 @@ void ErForceSimulator::setRobots( { if (side == gameController::Team::BLUE) { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); blue_primitive_executor_map.insert({id, robot_primitive_executor}); } else { - auto robot_primitive_executor = std::make_shared( - Duration::fromSeconds(primitive_executor_time_step_s), robot_constants); + auto robot_primitive_executor = + std::make_shared(robot_constants); yellow_primitive_executor_map.insert({id, robot_primitive_executor}); } } @@ -389,15 +389,17 @@ SSLSimulationProto::RobotControl ErForceSimulator::updateSimulatorRobots( TbotsProto::PrimitiveExecutorStatus status; // Added for compilation if (ramping) { - auto direct_control_no_ramp = primitive_executor->stepPrimitive(status); - direct_control = getRampedVelocityPrimitive( - current_velocity_map.at(robot_id).first, - current_velocity_map.at(robot_id).second, *direct_control_no_ramp, - primitive_executor_time_step_s); + auto direct_control_no_ramp = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); + direct_control = getRampedVelocityPrimitive( + current_velocity_map.at(robot_id).first, + current_velocity_map.at(robot_id).second, *direct_control_no_ramp, + primitive_executor_time_step); } else { - direct_control = primitive_executor->stepPrimitive(status); + direct_control = + primitive_executor->stepPrimitive(status, primitive_executor_time_step); } auto command = *getRobotCommandFromDirectControl( @@ -411,8 +413,7 @@ std::unique_ptr ErForceSimulator::getRampedVelocityPrimitive( const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, - TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp) + TbotsProto::DirectControlPrimitive& target_velocity_primitive, Duration time_to_ramp) { TbotsProto::MotorControl_DirectVelocityControl direct_velocity = target_velocity_primitive.motor_control().direct_velocity_control(); @@ -435,7 +436,7 @@ ErForceSimulator::getRampedVelocityPrimitive( euclidean_to_four_wheel.getWheelVelocity(current_euclidean_velocity); WheelSpace_t ramped_four_wheel = euclidean_to_four_wheel.rampWheelVelocity( - current_wheel_velocity, target_wheel_velocity, time_to_ramp); + current_wheel_velocity, target_wheel_velocity, time_to_ramp.toSeconds()); EuclideanSpace_t ramped_euclidean = euclidean_to_four_wheel.getEuclideanVelocity(ramped_four_wheel); diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index 80459c4df4..429b16d503 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -27,12 +27,12 @@ class ErForceSimulator * @param robot_constants The robot constants * @param realism_config realism configuration */ - explicit ErForceSimulator(const TbotsProto::FieldType& field_type, - const robot_constants::RobotConstants& robot_constants, - std::unique_ptr& realism_config, - const bool ramping = false, - double primitive_executor_time_step_s = - DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK); + explicit ErForceSimulator( + const TbotsProto::FieldType& field_type, + const robot_constants::RobotConstants& robot_constants, + std::unique_ptr& realism_config, const bool ramping = false, + Duration primitive_executor_time_step_s = + Duration::fromSeconds(DEFAULT_SIMULATOR_TICK_RATE_SECONDS_PER_TICK)); ErForceSimulator() = delete; ~ErForceSimulator() = default; @@ -202,7 +202,7 @@ class ErForceSimulator const Vector current_local_velocity, const AngularVelocity current_local_angular_velocity, TbotsProto::DirectControlPrimitive& target_velocity_primitive, - const double& time_to_ramp); + Duration time_to_ramp); // Map of Robot id to Primitive Executor std::unordered_map> @@ -212,7 +212,7 @@ class ErForceSimulator std::unique_ptr yellow_team_world_msg; std::unique_ptr blue_team_world_msg; - double primitive_executor_time_step_s; + Duration primitive_executor_time_step; unsigned int frame_number; // The current time.