Exit condition refactor

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"

include/VOSS/exit_conditions/ExitConditions.hpp

@@ -10,33 +10,66 @@
 namespace voss::controller {
 
 class ExitConditions : public AbstractExitCondition {
-
-  private:
-    std::vector<std::shared_ptr<controller::AbstractExitCondition>> conditions;
-    ExitConditions();
+    friend class ExitConditionsBuilder;
 
   public:
-    static ExitConditions new_conditions();
+    std::shared_ptr<ExitConditions>
+    set_settle(int settle_time, double tolerance, int initial_delay);
+    std::shared_ptr<ExitConditions> set_timeout(int timeout);
+    std::shared_ptr<ExitConditions> 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<bool()> callback);
-    ExitConditions& add_condition(std::shared_ptr<AbstractExitCondition> ec);
+    std::shared_ptr<ExitConditions>
+    set_linear_tolerance(double linear_tolerance, double tolerance_time);
+    std::shared_ptr<ExitConditions>
+    set_angular_tolerance(double angular_tolerance, double tolerance_time);
+    std::shared_ptr<ExitConditions> set_thru_smoothness(double smoothness);
+
+    template <class EC_TYPE>
+        requires(std::is_base_of<AbstractExitCondition, EC_TYPE>::value)
+    std::shared_ptr<ExitConditions> set_condition(EC_TYPE ec,
+                                                  bool replace_old = false) {
+        this->p = std::make_shared<ExitConditions>(*this);
+        if (!replace_old) {
+            p->conditions.push_back(std::make_shared<EC_TYPE>(ec));
+            return p;
+        }
+        auto opt = this->ec_is_repeated<EC_TYPE>();
+        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<bool()> 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<ExitConditions> build();
-
     void reset() override;
+
+  private:
+    ExitConditions();
+    template <class T> std::optional<size_t> ec_is_repeated() {
+        auto it =
+            std::find_if(conditions.cbegin(), conditions.cend(),
+                         [](const std::shared_ptr<AbstractExitCondition> ec) {
+                             return std::dynamic_pointer_cast<T>(ec) != nullptr;
+                         });
+        if (it == conditions.cend()) {
+            return std::nullopt;
+        }
+        return std::distance(conditions.cbegin(), it);
+    };
+
+  private:
+    std::shared_ptr<ExitConditions> p;
+    std::vector<std::shared_ptr<controller::AbstractExitCondition>> conditions;
 };
 
 } // namespace voss::controller

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<bool()> callback);
+    ExitConditionsBuilder& add_condition(std::shared_ptr<AbstractExitCondition> ec);
+
+    std::shared_ptr<ExitConditions> build();
+
+
+
+  private:
+    ExitConditionsBuilder() = default;
+    std::shared_ptr<ExitConditions> ec_ptr{};
+};
+}; // namespace voss::controller

src/VOSS/chassis/DiffChassis.cpp

@@ -96,7 +96,7 @@ DiffChassis::DiffChassis(
     this->left_motors = std::make_unique<pros::MotorGroup>(left_motors);
     this->right_motors = std::make_unique<pros::MotorGroup>(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);

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<SettleExitCondition>(ec));
-    return *this;
+std::shared_ptr<ExitConditions> ExitConditions::set_settle(int settle_time,
+                                                           double tolerance,
+                                                           int initial_delay) {
+    auto ec = std::make_shared<SettleExitCondition>(settle_time, tolerance,
+                                                    initial_delay);
+    auto opt = this->ec_is_repeated<SettleExitCondition>();
+    this->p = std::make_shared<ExitConditions>(*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<TimeOutExitCondition>(timeout));
-    return *this;
+std::shared_ptr<ExitConditions> ExitConditions::set_timeout(int timeout) {
+    auto ec = std::make_shared<TimeOutExitCondition>(timeout);
+    auto opt = this->ec_is_repeated<TimeOutExitCondition>();
+    this->p = std::make_shared<ExitConditions>(*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>
+ExitConditions::set_tolerance(double linear_tolerance, double angular_tolerance,
+                              double tolerance_time) {
     auto ec = std::make_shared<ToleranceExitCondition>();
     ec->add_lin_exit(linear_tolerance, tolerance_time);
     ec->add_ang_exit(angular_tolerance, tolerance_time);
-    this->conditions.push_back(
-        std::dynamic_pointer_cast<AbstractExitCondition>(ec));
-    return *this;
+
+    auto opt = this->ec_is_repeated<ToleranceExitCondition>();
+    this->p = std::make_shared<ExitConditions>(*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>
+ExitConditions::set_linear_tolerance(double linear_tolerance,
+                                     double tolerance_time) {
+    this->p = std::make_shared<ExitConditions>(*this);
+    auto opt = this->ec_is_repeated<ToleranceExitCondition>();
+    if (opt.has_value()) {
+        std::dynamic_pointer_cast<ToleranceExitCondition>(
+            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<PrepLineExitCondition>(smoothness));
-    return *this;
+std::shared_ptr<ExitConditions>
+ExitConditions::set_angular_tolerance(double angular_tolerance,
+                                      double tolerance_time) {
+    this->p = std::make_shared<ExitConditions>(*this);
+    auto opt = this->ec_is_repeated<ToleranceExitCondition>();
+    if (opt.has_value()) {
+        std::dynamic_pointer_cast<ToleranceExitCondition>(
+            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<bool()> callback) {
-    this->conditions.push_back(std::make_shared<CustomExitCondition>(callback));
-    return *this;
+std::shared_ptr<ExitConditions>
+ExitConditions::set_thru_smoothness(double smoothness) {
+    auto ec = std::make_shared<PrepLineExitCondition>(smoothness);
+
+    auto opt = this->ec_is_repeated<PrepLineExitCondition>();
+    this->p = std::make_shared<ExitConditions>(*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>
 ExitConditions::exit_if(std::function<bool()> callback) {
-    std::shared_ptr<ExitConditions> ec_mod =
-        std::make_shared<ExitConditions>(*this);
-    ec_mod->conditions.push_back(
+    this->p = std::make_shared<ExitConditions>(*this);
+    this->p->conditions.push_back(
         std::make_shared<CustomExitCondition>(callback));
-    return ec_mod;
-}
-
-ExitConditions&
-ExitConditions::add_condition(std::shared_ptr<AbstractExitCondition> 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> ExitConditions::build() {
-    return std::make_shared<ExitConditions>(*this);
-}
-
 void ExitConditions::reset() {
     for (auto ec : this->conditions) {
         ec->reset();

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<SettleExitCondition>(ec));
+    return *this;
+}
+
+ExitConditionsBuilder& ExitConditionsBuilder::add_timeout(int timeout) {
+    ec_ptr->conditions.push_back(std::make_shared<TimeOutExitCondition>(timeout));
+    return *this;
+}
+
+ExitConditionsBuilder& ExitConditionsBuilder::add_tolerance(double linear_tolerance,
+                                              double angular_tolerance,
+                                              double tolerance_time) {
+    auto ec = std::make_shared<ToleranceExitCondition>();
+    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<AbstractExitCondition>(ec));
+    return *this;
+}
+
+ExitConditionsBuilder& ExitConditionsBuilder::add_thru_smoothness(double smoothness) {
+    ec_ptr->conditions.push_back(
+        std::make_shared<PrepLineExitCondition>(smoothness));
+    return *this;
+}
+
+ExitConditionsBuilder&
+ExitConditionsBuilder::add_custom_condition(std::function<bool()> callback) {
+    ec_ptr->conditions.push_back(std::make_shared<CustomExitCondition>(callback));
+    return *this;
+}
+
+ExitConditionsBuilder&
+ExitConditionsBuilder::add_condition(std::shared_ptr<AbstractExitCondition> ec) {
+    ec_ptr->conditions.push_back(ec);
+    return *this;
+}
+std::shared_ptr<ExitConditions> ExitConditionsBuilder::build() {
+    return ec_ptr;
+}
+};

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);
-
 }