Add Exit Condition Class (#81)

--------- Co-authored-by: cjsport11 <[email protected]> Co-authored-by: Brandon-Liu <[email protected]> Co-authored-by: Mihir Laud <[email protected]> Co-authored-by: Rocky14683 <[email protected]> Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com>
purduesigbots · Mar 19, 2024 · 89a77d1 · 89a77d1
1 parent 168215d
commit 89a77d1
Show file tree

Hide file tree

Showing 41 changed files with 776 additions and 430 deletions.
diff --git a/include/VOSS/api.hpp b/include/VOSS/api.hpp
@@ -16,6 +16,13 @@
 #include "controller/SwingController.hpp"
 #include "controller/SwingControllerBuilder.hpp"
 
+#include "exit_conditions/AbstractExitCondition.hpp"
+#include "exit_conditions/ExitConditions.hpp"
+#include "exit_conditions/SettleExitCondition.hpp"
+#include "exit_conditions/TimeOutExitCondition.hpp"
+#include "exit_conditions/ToleranceAngularExitCondition.hpp"
+#include "exit_conditions/ToleranceLinearExitCondition.hpp"
+
 #include "localizer/AbstractLocalizer.hpp"
 #include "localizer/ADILocalizer.hpp"
 #include "localizer/ADILocalizerBuilder.hpp"

diff --git a/include/VOSS/chassis/AbstractChassis.hpp b/include/VOSS/chassis/AbstractChassis.hpp
@@ -5,6 +5,7 @@
 
 #include "ChassisCommand.hpp"
 #include "VOSS/controller/AbstractController.hpp"
+#include "VOSS/exit_conditions/AbstractExitCondition.hpp"
 
 #include "VOSS/utils/flags.hpp"
 #include "VOSS/utils/Point.hpp"
@@ -13,56 +14,66 @@
 namespace voss::chassis {
 
 using controller_ptr = std::shared_ptr<controller::AbstractController>;
+using ec_ptr = std::shared_ptr<controller::AbstractExitCondition>;
 
 class AbstractChassis {
   protected:
     controller_ptr default_controller;
+    ec_ptr default_ec;
     std::unique_ptr<pros::Task> task = nullptr;
     bool task_running = false;
     pros::motor_brake_mode_e brakeMode;
 
-    void move_task(controller_ptr controller, double max, voss::Flags flags,
-                   double exitTime);
+    void move_task(controller_ptr controller, ec_ptr ec, double max,
+                   voss::Flags flags);
 
-    void turn_task(controller_ptr controller, double max, voss::Flags flags,
-                   voss::AngularDirection direction, double exitTime);
+    void turn_task(controller_ptr controller, ec_ptr ec, double max,
+                   voss::Flags flags, voss::AngularDirection direction);
 
   public:
-    AbstractChassis(controller_ptr default_controller);
+    AbstractChassis(controller_ptr default_controller, ec_ptr ec);
 
     virtual void tank(double left_speed, double right_speed) = 0;
     virtual void arcade(double forward_speed, double turn_speed) = 0;
 
     virtual bool execute(DiffChassisCommand cmd, double max) = 0;
     virtual void set_brake_mode(pros::motor_brake_mode_e mode) = 0;
 
+    void move(Pose target, controller_ptr controller, ec_ptr ec,
+              double max = 100.0, voss::Flags flags = voss::Flags::NONE);
+
     void move(Pose target, controller_ptr controller, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
+              voss::Flags flags = voss::Flags::NONE);
 
     void move(Pose target, double max = 100.0,
-              voss::Flags flags = voss::Flags::NONE, double exitTime = 22500);
+              voss::Flags flags = voss::Flags::NONE);
+
+    void turn(double target, controller_ptr controller, ec_ptr ec,
+              double max = 100.0, voss::Flags flags = voss::Flags::NONE,
+              voss::AngularDirection direction = voss::AngularDirection::AUTO);
 
     void turn(double target, controller_ptr controller, double max = 100.0,
               voss::Flags flags = voss::Flags::NONE,
-              voss::AngularDirection direction = voss::AngularDirection::AUTO,
-              double exitTime = 22500);
+              voss::AngularDirection direction = voss::AngularDirection::AUTO);
 
     void turn(double target, double max = 100.0,
               voss::Flags flags = voss::Flags::NONE,
-              voss::AngularDirection direction = voss::AngularDirection::AUTO,
-              double exitTime = 22500);
+              voss::AngularDirection direction = voss::AngularDirection::AUTO);
+
+    void
+    turn_to(Point target, controller_ptr controller, ec_ptr ec,
+            double max = 100.0, voss::Flags flags = voss::Flags::NONE,
+            voss::AngularDirection direction = voss::AngularDirection::AUTO);
 
     void
     turn_to(Point target, controller_ptr controller, double max = 100.0,
             voss::Flags flags = voss::Flags::NONE,
-            voss::AngularDirection direction = voss::AngularDirection::AUTO,
-            double exitTime = 22500);
+            voss::AngularDirection direction = voss::AngularDirection::AUTO);
 
     void
     turn_to(Point target, double max = 100.0,
             voss::Flags flags = voss::Flags::NONE,
-            voss::AngularDirection direction = voss::AngularDirection::AUTO,
-            double exitTime = 22500);
+            voss::AngularDirection direction = voss::AngularDirection::AUTO);
 };
 
 } // namespace voss::chassis
diff --git a/include/VOSS/chassis/DiffChassis.hpp b/include/VOSS/chassis/DiffChassis.hpp
@@ -21,7 +21,8 @@ class DiffChassis : public AbstractChassis {
   public:
     DiffChassis(std::initializer_list<int8_t> left_motors,
                 std::initializer_list<int8_t> right_motors,
-                controller_ptr default_controller, double slew_step = 8,
+                controller_ptr default_controller, ec_ptr ec,
+                double slew_step = 8,
                 pros::motor_brake_mode_e brakeMode =
                     pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST);
 

diff --git a/include/VOSS/controller/AbstractController.hpp b/include/VOSS/controller/AbstractController.hpp
@@ -1,6 +1,7 @@
 #pragma once
 
 #include "VOSS/chassis/ChassisCommand.hpp"
+#include "VOSS/exit_conditions/AbstractExitCondition.hpp"
 #include "VOSS/localizer/AbstractLocalizer.hpp"
 #include "VOSS/utils/flags.hpp"
 
@@ -16,15 +17,18 @@ class AbstractController {
   public:
     AbstractController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
-    virtual chassis::DiffChassisCommand get_command(bool reverse,
-                                                    bool thru) = 0;
+    virtual chassis::DiffChassisCommand
+    get_command(bool reverse, bool thru,
+                std::shared_ptr<AbstractExitCondition> ec) = 0;
     virtual chassis::DiffChassisCommand
     get_angular_command(bool reverse, bool thru,
-                        voss::AngularDirection direction) = 0;
+                        voss::AngularDirection direction,
+                        std::shared_ptr<AbstractExitCondition> ec) = 0;
 
     virtual void reset() = 0;
 
-    void set_target(Pose target, bool relative);
+    void set_target(Pose target, bool relative,
+                    std::shared_ptr<AbstractExitCondition> ec);
     void set_angular_target(double angle, bool relative);
 };
 

diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp
@@ -10,10 +10,8 @@ class ArcPIDController : public AbstractController {
     std::shared_ptr<ArcPIDController> p;
     double linear_kP, linear_kI, linear_kD;
     double track_width;
-    double exit_error;
     double min_error;
     double can_reverse;
-    double settle_time;
     double prev_t;
     double slew;
     double prev_lin_speed;
@@ -27,19 +25,20 @@ class ArcPIDController : public AbstractController {
 
     double linear_pid(double error);
 
-    chassis::DiffChassisCommand get_command(bool reverse, bool thru) override;
+    chassis::DiffChassisCommand
+    get_command(bool reverse, bool thru,
+                std::shared_ptr<AbstractExitCondition> ec) override;
     chassis::DiffChassisCommand
     get_angular_command(bool reverse, bool thru,
-                        voss::AngularDirection direction) override;
+                        voss::AngularDirection direction,
+                        std::shared_ptr<AbstractExitCondition> ec) override;
 
     void reset() override;
 
     std::shared_ptr<ArcPIDController>
     modify_linear_constants(double kP, double kI, double kD);
     std::shared_ptr<ArcPIDController> modify_track_width(double track_width);
-    std::shared_ptr<ArcPIDController> modify_exit_error(double error);
     std::shared_ptr<ArcPIDController> modify_min_error(double error);
-    std::shared_ptr<ArcPIDController> modify_settle_time(double time);
     std::shared_ptr<ArcPIDController> modify_slew(double slew);
 
     friend class ArcPIDControllerBuilder;

diff --git a/include/VOSS/controller/ArcPIDControllerBuilder.hpp b/include/VOSS/controller/ArcPIDControllerBuilder.hpp
@@ -24,9 +24,7 @@ class ArcPIDControllerBuilder {
      * than the real one.
      */
     ArcPIDControllerBuilder& with_track_width(double track_width);
-    ArcPIDControllerBuilder& with_exit_error(double error);
     ArcPIDControllerBuilder& with_min_error(double error);
-    ArcPIDControllerBuilder& with_settle_time(double time);
     ArcPIDControllerBuilder& with_slew(double slew);
 
     std::shared_ptr<ArcPIDController> build();

diff --git a/include/VOSS/controller/BoomerangController.hpp b/include/VOSS/controller/BoomerangController.hpp
@@ -3,6 +3,7 @@
 #include "AbstractController.hpp"
 #include "PIDController.hpp"
 #include "VOSS/chassis/ChassisCommand.hpp"
+#include "VOSS/exit_conditions/AbstractExitCondition.hpp"
 #include "VOSS/localizer/AbstractLocalizer.hpp"
 #include "VOSS/utils/flags.hpp"
 
@@ -34,10 +35,13 @@ class BoomerangController : public AbstractController {
   public:
     BoomerangController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
-    chassis::DiffChassisCommand get_command(bool reverse, bool thru) override;
+    chassis::DiffChassisCommand
+    get_command(bool reverse, bool thru,
+                std::shared_ptr<AbstractExitCondition> ec) override;
     chassis::DiffChassisCommand
     get_angular_command(bool reverse, bool thru,
-                        voss::AngularDirection direction) override;
+                        voss::AngularDirection direction,
+                        std::shared_ptr<AbstractExitCondition> ec) override;
 
     double linear_pid(double error);
     double angular_pid(double error);

diff --git a/include/VOSS/controller/PIDController.hpp b/include/VOSS/controller/PIDController.hpp
@@ -12,16 +12,9 @@ class PIDController : public AbstractController {
     double linear_kP, linear_kI, linear_kD;
     double angular_kP, angular_kI, angular_kD;
     double tracking_kP;
-    double exit_error;
-    double angular_exit_error;
     double min_error;
     bool can_reverse;
-    double settle_time;
 
-    double close;
-    double close_2;
-    int counter;
-    double prev_angle;
     double min_vel;
     bool turn_overshoot;
 
@@ -33,10 +26,13 @@ class PIDController : public AbstractController {
     double linear_pid(double error);
     double angular_pid(double error);
 
-    chassis::DiffChassisCommand get_command(bool reverse, bool thru) override;
+    chassis::DiffChassisCommand
+    get_command(bool reverse, bool thru,
+                std::shared_ptr<AbstractExitCondition> ec) override;
     chassis::DiffChassisCommand
     get_angular_command(bool reverse, bool thru,
-                        voss::AngularDirection direction) override;
+                        voss::AngularDirection direction,
+                        std::shared_ptr<AbstractExitCondition> ec) override;
 
     void reset() override;
 

diff --git a/include/VOSS/controller/PIDControllerBuilder.hpp b/include/VOSS/controller/PIDControllerBuilder.hpp
@@ -21,10 +21,7 @@ class PIDControllerBuilder {
     PIDControllerBuilder& with_angular_constants(double kP, double kI,
                                                  double kD);
     PIDControllerBuilder& with_tracking_kp(double kP);
-    PIDControllerBuilder& with_exit_error(double error);
-    PIDControllerBuilder& with_angular_exit_error(double error);
     PIDControllerBuilder& with_min_error(double error);
-    PIDControllerBuilder& with_settle_time(double time);
     PIDControllerBuilder& with_min_vel_for_thru(double min_vel);
 
     std::shared_ptr<PIDController> build();

diff --git a/include/VOSS/controller/SwingController.hpp b/include/VOSS/controller/SwingController.hpp
@@ -9,13 +9,7 @@ class SwingController : public AbstractController {
   protected:
     std::shared_ptr<SwingController> p;
     double angular_kP, angular_kI, angular_kD;
-    double angular_exit_error;
-    double settle_time;
     bool can_reverse;
-
-    double close;
-    double close_2;
-    int counter;
     bool turn_overshoot;
 
     double prev_ang_err, total_ang_err;
@@ -24,10 +18,13 @@ class SwingController : public AbstractController {
   public:
     SwingController(std::shared_ptr<localizer::AbstractLocalizer> l);
 
-    chassis::DiffChassisCommand get_command(bool reverse, bool thru) override;
+    chassis::DiffChassisCommand
+    get_command(bool reverse, bool thru,
+                std::shared_ptr<AbstractExitCondition> ec) override;
     chassis::DiffChassisCommand
     get_angular_command(bool reverse, bool thru,
-                        voss::AngularDirection direction) override;
+                        voss::AngularDirection direction,
+                        std::shared_ptr<AbstractExitCondition> ec) override;
 
     double angular_pid(double error);
 

diff --git a/include/VOSS/controller/SwingControllerBuilder.hpp b/include/VOSS/controller/SwingControllerBuilder.hpp
@@ -20,9 +20,6 @@ class SwingControllerBuilder {
 
     SwingControllerBuilder& with_angular_constants(double kP, double kI,
                                                    double kD);
-    SwingControllerBuilder& with_angular_exit_error(double error);
-    SwingControllerBuilder& with_settle_time(double time);
-    //    SwingControllerBuilder& with_slew(double slew);
 
     std::shared_ptr<SwingController> build();
 };

diff --git a/include/VOSS/exit_conditions/AbstractExitCondition.hpp b/include/VOSS/exit_conditions/AbstractExitCondition.hpp
@@ -0,0 +1,20 @@
+#pragma once
+
+#include "VOSS/utils/Pose.hpp"
+
+namespace voss::controller {
+
+class AbstractExitCondition {
+
+  protected:
+    Pose target_pose;
+    bool target_has_coordinate() const;
+    bool target_has_heading() const;
+
+  public:
+    virtual bool is_met(Pose current_pose, bool thru) = 0;
+    virtual void set_target(Pose target);
+    virtual void reset();
+};
+
+} // namespace voss::controller
diff --git a/include/VOSS/exit_conditions/CustomExitCondition.hpp b/include/VOSS/exit_conditions/CustomExitCondition.hpp
@@ -0,0 +1,14 @@
+#pragma once
+#include "../utils/Pose.hpp"
+#include "AbstractExitCondition.hpp"
+#include <functional>
+namespace voss::controller {
+class CustomExitCondition : public AbstractExitCondition {
+  private:
+    std::function<bool()> callback;
+
+  public:
+    CustomExitCondition(std::function<bool()> callback);
+    bool is_met(Pose current_pose, bool thru) override;
+};
+} // namespace voss::controller
diff --git a/include/VOSS/exit_conditions/ExitConditions.hpp b/include/VOSS/exit_conditions/ExitConditions.hpp
@@ -0,0 +1,41 @@
+#pragma once
+
+#include "AbstractExitCondition.hpp"
+#include "ToleranceExitCondition.hpp"
+#include "VOSS/utils/Pose.hpp"
+#include <functional>
+#include <memory>
+#include <vector>
+
+namespace voss::controller {
+
+class ExitConditions : public AbstractExitCondition {
+
+  private:
+    std::vector<std::shared_ptr<controller::AbstractExitCondition>> conditions;
+    ExitConditions();
+
+  public:
+    static ExitConditions new_conditions();
+
+    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);
+    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> exit_if(std::function<bool()> callback);
+
+    bool is_met(voss::Pose current_pose, bool thru);
+    bool all_met(voss::Pose current_pose, bool thru);
+
+    std::shared_ptr<ExitConditions> build();
+
+    void reset() override;
+};
+
+} // namespace voss::controller
diff --git a/include/VOSS/exit_conditions/PrepLineExitCondition.hpp b/include/VOSS/exit_conditions/PrepLineExitCondition.hpp
@@ -0,0 +1,14 @@
+#pragma once
+#include "AbstractExitCondition.hpp"
+#include "VOSS/utils/Pose.hpp"
+
+namespace voss::controller {
+class PrepLineExitCondition : public AbstractExitCondition {
+  private:
+    double thru_smoothness;
+
+  public:
+    PrepLineExitCondition(double thru_smoothness);
+    bool is_met(voss::Pose pose, bool thru) override;
+};
+} // namespace voss::controller