diff --git a/include/VOSS/api.hpp b/include/VOSS/api.hpp index 7ddb1854..1ae1e534 100644 --- a/include/VOSS/api.hpp +++ b/include/VOSS/api.hpp @@ -18,6 +18,7 @@ #include "exit_conditions/AbstractExitCondition.hpp" #include "exit_conditions/ExitConditions.hpp" +#include "exit_conditions/ExitConditionsBuilder.hpp" #include "exit_conditions/SettleExitCondition.hpp" #include "exit_conditions/TimeOutExitCondition.hpp" #include "exit_conditions/ToleranceAngularExitCondition.hpp" diff --git a/include/VOSS/exit_conditions/ExitConditions.hpp b/include/VOSS/exit_conditions/ExitConditions.hpp index 54f91536..7610a720 100644 --- a/include/VOSS/exit_conditions/ExitConditions.hpp +++ b/include/VOSS/exit_conditions/ExitConditions.hpp @@ -10,33 +10,66 @@ namespace voss::controller { class ExitConditions : public AbstractExitCondition { - - private: - std::vector> conditions; - ExitConditions(); + friend class ExitConditionsBuilder; public: - static ExitConditions new_conditions(); + std::shared_ptr + set_settle(int settle_time, double tolerance, int initial_delay); + std::shared_ptr set_timeout(int timeout); + std::shared_ptr set_tolerance(double linear_tolerance, + double angular_tolerance, + double tolerance_time); - void set_target(voss::Pose new_target) override; - ExitConditions& add_settle(int settle_time, double tolerance, - int initial_delay); - ExitConditions& add_timeout(int timeout); - ExitConditions& add_tolerance(double linear_tolerance, - double angular_tolerance, - double tolerance_time); - ExitConditions& add_thru_smoothness(double smoothness); - ExitConditions& add_custom_condition(std::function callback); - ExitConditions& add_condition(std::shared_ptr ec); + std::shared_ptr + set_linear_tolerance(double linear_tolerance, double tolerance_time); + std::shared_ptr + set_angular_tolerance(double angular_tolerance, double tolerance_time); + std::shared_ptr set_thru_smoothness(double smoothness); + + template + requires(std::is_base_of::value) + std::shared_ptr set_condition(EC_TYPE ec, + bool replace_old = false) { + this->p = std::make_shared(*this); + if (!replace_old) { + p->conditions.push_back(std::make_shared(ec)); + return p; + } + auto opt = this->ec_is_repeated(); + if (opt.has_value()) { + this->p->conditions.at(opt.value()) = std::move(ec); + } else { + this->p->conditions.push_back(ec); + } + + return this->p; + } std::shared_ptr exit_if(std::function callback); - bool is_met(voss::Pose current_pose, bool thru); + void set_target(voss::Pose new_target) override; + bool is_met(voss::Pose current_pose, bool thru) override; bool all_met(voss::Pose current_pose, bool thru); - std::shared_ptr build(); - void reset() override; + + private: + ExitConditions(); + template std::optional ec_is_repeated() { + auto it = + std::find_if(conditions.cbegin(), conditions.cend(), + [](const std::shared_ptr ec) { + return std::dynamic_pointer_cast(ec) != nullptr; + }); + if (it == conditions.cend()) { + return std::nullopt; + } + return std::distance(conditions.cbegin(), it); + }; + + private: + std::shared_ptr p; + std::vector> conditions; }; } // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/ExitConditionsBuilder.hpp b/include/VOSS/exit_conditions/ExitConditionsBuilder.hpp new file mode 100644 index 00000000..c6e2a220 --- /dev/null +++ b/include/VOSS/exit_conditions/ExitConditionsBuilder.hpp @@ -0,0 +1,27 @@ +#pragma once +#include "ExitConditions.hpp" + +namespace voss::controller { +class ExitConditionsBuilder { + public: + static ExitConditionsBuilder new_builder(); + + ExitConditionsBuilder& add_settle(int settle_time, double tolerance, + int initial_delay); + ExitConditionsBuilder& add_timeout(int timeout); + ExitConditionsBuilder& add_tolerance(double linear_tolerance, + double angular_tolerance, + double tolerance_time); + ExitConditionsBuilder& add_thru_smoothness(double smoothness); + ExitConditionsBuilder& add_custom_condition(std::function callback); + ExitConditionsBuilder& add_condition(std::shared_ptr ec); + + std::shared_ptr build(); + + + + private: + ExitConditionsBuilder() = default; + std::shared_ptr ec_ptr{}; +}; +}; // namespace voss::controller diff --git a/src/VOSS/chassis/DiffChassis.cpp b/src/VOSS/chassis/DiffChassis.cpp index 44032bfd..6ecadb81 100644 --- a/src/VOSS/chassis/DiffChassis.cpp +++ b/src/VOSS/chassis/DiffChassis.cpp @@ -96,7 +96,7 @@ DiffChassis::DiffChassis( this->left_motors = std::make_unique(left_motors); this->right_motors = std::make_unique(right_motors); - this->slew_step = slew_step > 0 ? slew_step : 200; + this->slew_step = slew_step >= 0 ? slew_step : 200; this->brakeMode = brakeMode; this->left_motors->set_brake_mode(this->brakeMode); this->right_motors->set_brake_mode(this->brakeMode); diff --git a/src/VOSS/exit_conditions/ExitConditions.cpp b/src/VOSS/exit_conditions/ExitConditions.cpp index 724a0ab2..bd291241 100644 --- a/src/VOSS/exit_conditions/ExitConditions.cpp +++ b/src/VOSS/exit_conditions/ExitConditions.cpp @@ -14,12 +14,6 @@ namespace voss::controller { ExitConditions::ExitConditions() { } -ExitConditions ExitConditions::new_conditions() { - ExitConditions ec; - - return ec; -} - void ExitConditions::set_target(voss::Pose new_target) { this->target_pose = new_target; @@ -28,54 +22,104 @@ void ExitConditions::set_target(voss::Pose new_target) { } } -ExitConditions& ExitConditions::add_settle(int settle_time, double tolerance, - int initial_delay) { - SettleExitCondition ec(settle_time, tolerance, initial_delay); - this->conditions.push_back(std::make_shared(ec)); - return *this; +std::shared_ptr ExitConditions::set_settle(int settle_time, + double tolerance, + int initial_delay) { + auto ec = std::make_shared(settle_time, tolerance, + initial_delay); + auto opt = this->ec_is_repeated(); + this->p = std::make_shared(*this); + if (opt.has_value()) { + this->p->conditions.at(opt.value()) = std::move(ec); + } else { + this->p->conditions.push_back(ec); + } + + return this->p; } -ExitConditions& ExitConditions::add_timeout(int timeout) { - this->conditions.push_back(std::make_shared(timeout)); - return *this; +std::shared_ptr ExitConditions::set_timeout(int timeout) { + auto ec = std::make_shared(timeout); + auto opt = this->ec_is_repeated(); + this->p = std::make_shared(*this); + if (opt.has_value()) { + this->p->conditions.at(opt.value()) = std::move(ec); + } else { + this->p->conditions.push_back(ec); + } + + return this->p; } -ExitConditions& ExitConditions::add_tolerance(double linear_tolerance, - double angular_tolerance, - double tolerance_time) { +std::shared_ptr +ExitConditions::set_tolerance(double linear_tolerance, double angular_tolerance, + double tolerance_time) { auto ec = std::make_shared(); ec->add_lin_exit(linear_tolerance, tolerance_time); ec->add_ang_exit(angular_tolerance, tolerance_time); - this->conditions.push_back( - std::dynamic_pointer_cast(ec)); - return *this; + + auto opt = this->ec_is_repeated(); + this->p = std::make_shared(*this); + if (opt.has_value()) { + this->p->conditions.at(opt.value()) = std::move(ec); + } else { + this->p->conditions.push_back(ec); + } + + return this->p; +} + +std::shared_ptr +ExitConditions::set_linear_tolerance(double linear_tolerance, + double tolerance_time) { + this->p = std::make_shared(*this); + auto opt = this->ec_is_repeated(); + if (opt.has_value()) { + std::dynamic_pointer_cast( + this->p->conditions.at(opt.value())) + ->add_lin_exit(linear_tolerance, tolerance_time); + } else { + this->p->set_tolerance(linear_tolerance, 0, tolerance_time); + } + return this->p; } -ExitConditions& ExitConditions::add_thru_smoothness(double smoothness) { - this->conditions.push_back( - std::make_shared(smoothness)); - return *this; +std::shared_ptr +ExitConditions::set_angular_tolerance(double angular_tolerance, + double tolerance_time) { + this->p = std::make_shared(*this); + auto opt = this->ec_is_repeated(); + if (opt.has_value()) { + std::dynamic_pointer_cast( + this->p->conditions.at(opt.value())) + ->add_ang_exit(angular_tolerance, tolerance_time); + } else { + this->p->set_tolerance(0, angular_tolerance, tolerance_time); + } + return this->p; } -ExitConditions& -ExitConditions::add_custom_condition(std::function callback) { - this->conditions.push_back(std::make_shared(callback)); - return *this; +std::shared_ptr +ExitConditions::set_thru_smoothness(double smoothness) { + auto ec = std::make_shared(smoothness); + + auto opt = this->ec_is_repeated(); + this->p = std::make_shared(*this); + if (opt.has_value()) { + this->p->conditions.at(opt.value()) = std::move(ec); + } else { + this->p->conditions.push_back(ec); + } + + return this->p; } std::shared_ptr ExitConditions::exit_if(std::function callback) { - std::shared_ptr ec_mod = - std::make_shared(*this); - ec_mod->conditions.push_back( + this->p = std::make_shared(*this); + this->p->conditions.push_back( std::make_shared(callback)); - return ec_mod; -} - -ExitConditions& -ExitConditions::add_condition(std::shared_ptr ec) { - this->conditions.push_back(ec); - return *this; + return p; } bool ExitConditions::is_met(voss::Pose current_pose, bool thru) { @@ -98,10 +142,6 @@ bool ExitConditions::all_met(voss::Pose current_pose, bool thru) { return true; } -std::shared_ptr ExitConditions::build() { - return std::make_shared(*this); -} - void ExitConditions::reset() { for (auto ec : this->conditions) { ec->reset(); diff --git a/src/VOSS/exit_conditions/ExitConditionsBuilder.cpp b/src/VOSS/exit_conditions/ExitConditionsBuilder.cpp new file mode 100644 index 00000000..74d92ee6 --- /dev/null +++ b/src/VOSS/exit_conditions/ExitConditionsBuilder.cpp @@ -0,0 +1,55 @@ +#include "VOSS/exit_conditions/ExitConditionsBuilder.hpp" +#include "CustomExitCondition.hpp" +#include "PrepLineExitCondition.hpp" +#include "SettleExitCondition.hpp" +#include "TimeOutExitCondition.hpp" + +namespace voss::controller { +ExitConditionsBuilder ExitConditionsBuilder::new_builder() { + return {}; +} + +ExitConditionsBuilder& ExitConditionsBuilder::add_settle(int settle_time, double tolerance, + int initial_delay) { + SettleExitCondition ec(settle_time, tolerance, initial_delay); + ec_ptr->conditions.push_back(std::make_shared(ec)); + return *this; +} + +ExitConditionsBuilder& ExitConditionsBuilder::add_timeout(int timeout) { + ec_ptr->conditions.push_back(std::make_shared(timeout)); + return *this; +} + +ExitConditionsBuilder& ExitConditionsBuilder::add_tolerance(double linear_tolerance, + double angular_tolerance, + double tolerance_time) { + auto ec = std::make_shared(); + ec->add_lin_exit(linear_tolerance, tolerance_time); + ec->add_ang_exit(angular_tolerance, tolerance_time); + ec_ptr->conditions.push_back( + std::dynamic_pointer_cast(ec)); + return *this; +} + +ExitConditionsBuilder& ExitConditionsBuilder::add_thru_smoothness(double smoothness) { + ec_ptr->conditions.push_back( + std::make_shared(smoothness)); + return *this; +} + +ExitConditionsBuilder& +ExitConditionsBuilder::add_custom_condition(std::function callback) { + ec_ptr->conditions.push_back(std::make_shared(callback)); + return *this; +} + +ExitConditionsBuilder& +ExitConditionsBuilder::add_condition(std::shared_ptr ec) { + ec_ptr->conditions.push_back(ec); + return *this; +} +std::shared_ptr ExitConditionsBuilder::build() { + return ec_ptr; +} +}; \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 2a3a005e..33d48f82 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -42,7 +42,7 @@ auto traj_constraints = voss::trajectory::TrajectoryConstraints { }; pros::Controller master(pros::E_CONTROLLER_MASTER); -auto ec = voss::controller::ExitConditions::new_conditions() +auto ec = voss::controller::ExitConditionsBuilder::new_builder() .add_settle(400, 0.5, 400) .add_tolerance(1.0, 2.0, 200) .add_timeout(22500) @@ -131,5 +131,4 @@ ASSET(traj_txt) void opcontrol() { odom->set_pose({0.0, 0.0, 90}); chassis.follow_trajectory({{0, 0, 0}, {20, 20, 5}, {45, 25, 180}}, ramsete, traj_constraints); - } \ No newline at end of file