From 89a77d1df2d78e5d24bccf72c214115e338a8167 Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Tue, 19 Mar 2024 18:41:18 -0400 Subject: [PATCH] Add Exit Condition Class (#81) --------- Co-authored-by: cjsport11 Co-authored-by: Brandon-Liu Co-authored-by: Mihir Laud Co-authored-by: Rocky14683 Co-authored-by: github-actions <41898282+github-actions[bot]@users.noreply.github.com> --- include/VOSS/api.hpp | 7 + include/VOSS/chassis/AbstractChassis.hpp | 41 +++--- include/VOSS/chassis/DiffChassis.hpp | 3 +- .../VOSS/controller/AbstractController.hpp | 12 +- include/VOSS/controller/ArcPIDController.hpp | 11 +- .../controller/ArcPIDControllerBuilder.hpp | 2 - .../VOSS/controller/BoomerangController.hpp | 8 +- include/VOSS/controller/PIDController.hpp | 14 +- .../VOSS/controller/PIDControllerBuilder.hpp | 3 - include/VOSS/controller/SwingController.hpp | 13 +- .../controller/SwingControllerBuilder.hpp | 3 - .../exit_conditions/AbstractExitCondition.hpp | 20 +++ .../exit_conditions/CustomExitCondition.hpp | 14 ++ .../VOSS/exit_conditions/ExitConditions.hpp | 41 ++++++ .../exit_conditions/PrepLineExitCondition.hpp | 14 ++ .../exit_conditions/SettleExitCondition.hpp | 22 ++++ .../exit_conditions/TimeOutExitCondition.hpp | 18 +++ .../ToleranceAngularExitCondition.hpp | 16 +++ .../ToleranceExitCondition.hpp | 18 +++ .../ToleranceLinearExitCondition.hpp | 17 +++ include/VOSS/utils/Pose.hpp | 2 + src/VOSS/chassis/AbstractChassis.cpp | 88 ++++++++----- src/VOSS/chassis/DiffChassis.cpp | 6 +- src/VOSS/controller/AbstractController.cpp | 7 +- src/VOSS/controller/ArcPIDController.cpp | 52 ++------ .../controller/ArcPIDControllerBuilder.cpp | 12 -- src/VOSS/controller/BoomerangController.cpp | 106 ++++----------- src/VOSS/controller/PIDController.cpp | 122 ++++-------------- src/VOSS/controller/PIDControllerBuilder.cpp | 16 --- src/VOSS/controller/SwingController.cpp | 104 +++++---------- .../controller/SwingControllerBuilder.cpp | 11 -- .../exit_conditions/AbstractExitCondition.cpp | 19 +++ .../exit_conditions/CustomExitCondition.cpp | 13 ++ src/VOSS/exit_conditions/ExitConditions.cpp | 110 ++++++++++++++++ .../exit_conditions/PrepLineExitCondition.cpp | 48 +++++++ .../exit_conditions/SettleExitCondition.cpp | 56 ++++++++ .../exit_conditions/TimeOutExitCondition.cpp | 21 +++ .../ToleranceAngularExitCondition.cpp | 21 +++ .../ToleranceExitCondition.cpp | 25 ++++ .../ToleranceLinearExitCondition.cpp | 22 ++++ src/main.cpp | 48 ++++--- 41 files changed, 776 insertions(+), 430 deletions(-) create mode 100644 include/VOSS/exit_conditions/AbstractExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/CustomExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/ExitConditions.hpp create mode 100644 include/VOSS/exit_conditions/PrepLineExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/SettleExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/TimeOutExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/ToleranceAngularExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/ToleranceExitCondition.hpp create mode 100644 include/VOSS/exit_conditions/ToleranceLinearExitCondition.hpp create mode 100644 src/VOSS/exit_conditions/AbstractExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/CustomExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/ExitConditions.cpp create mode 100644 src/VOSS/exit_conditions/PrepLineExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/SettleExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/TimeOutExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/ToleranceExitCondition.cpp create mode 100644 src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp diff --git a/include/VOSS/api.hpp b/include/VOSS/api.hpp index b2fcf07..7fc5501 100644 --- 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 index 8c282e5..ba26810 100644 --- 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,22 +14,24 @@ namespace voss::chassis { using controller_ptr = std::shared_ptr; +using ec_ptr = std::shared_ptr; class AbstractChassis { protected: controller_ptr default_controller; + ec_ptr default_ec; std::unique_ptr 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; @@ -36,33 +39,41 @@ class AbstractChassis { 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 \ No newline at end of file diff --git a/include/VOSS/chassis/DiffChassis.hpp b/include/VOSS/chassis/DiffChassis.hpp index 3278b7f..608987d 100644 --- 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 left_motors, std::initializer_list 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 index 616058a..bd4a378 100644 --- 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 l); - virtual chassis::DiffChassisCommand get_command(bool reverse, - bool thru) = 0; + virtual chassis::DiffChassisCommand + get_command(bool reverse, bool thru, + std::shared_ptr ec) = 0; virtual chassis::DiffChassisCommand get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) = 0; + voss::AngularDirection direction, + std::shared_ptr ec) = 0; virtual void reset() = 0; - void set_target(Pose target, bool relative); + void set_target(Pose target, bool relative, + std::shared_ptr ec); void set_angular_target(double angle, bool relative); }; diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp index a75c0b2..94c13e6 100644 --- a/include/VOSS/controller/ArcPIDController.hpp +++ b/include/VOSS/controller/ArcPIDController.hpp @@ -10,10 +10,8 @@ class ArcPIDController : public AbstractController { std::shared_ptr 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 ec) override; chassis::DiffChassisCommand get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) override; + voss::AngularDirection direction, + std::shared_ptr ec) override; void reset() override; std::shared_ptr modify_linear_constants(double kP, double kI, double kD); std::shared_ptr modify_track_width(double track_width); - std::shared_ptr modify_exit_error(double error); std::shared_ptr modify_min_error(double error); - std::shared_ptr modify_settle_time(double time); std::shared_ptr modify_slew(double slew); friend class ArcPIDControllerBuilder; diff --git a/include/VOSS/controller/ArcPIDControllerBuilder.hpp b/include/VOSS/controller/ArcPIDControllerBuilder.hpp index 25b51c4..cdfb9a0 100644 --- 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 build(); diff --git a/include/VOSS/controller/BoomerangController.hpp b/include/VOSS/controller/BoomerangController.hpp index bdd1b6b..316e692 100644 --- 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 l); - chassis::DiffChassisCommand get_command(bool reverse, bool thru) override; + chassis::DiffChassisCommand + get_command(bool reverse, bool thru, + std::shared_ptr ec) override; chassis::DiffChassisCommand get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) override; + voss::AngularDirection direction, + std::shared_ptr 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 index b1ef05a..f92d985 100644 --- 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 ec) override; chassis::DiffChassisCommand get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) override; + voss::AngularDirection direction, + std::shared_ptr ec) override; void reset() override; diff --git a/include/VOSS/controller/PIDControllerBuilder.hpp b/include/VOSS/controller/PIDControllerBuilder.hpp index 7544adb..8bc6d7c 100644 --- 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 build(); diff --git a/include/VOSS/controller/SwingController.hpp b/include/VOSS/controller/SwingController.hpp index 421a206..2d38c81 100644 --- a/include/VOSS/controller/SwingController.hpp +++ b/include/VOSS/controller/SwingController.hpp @@ -9,13 +9,7 @@ class SwingController : public AbstractController { protected: std::shared_ptr 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 l); - chassis::DiffChassisCommand get_command(bool reverse, bool thru) override; + chassis::DiffChassisCommand + get_command(bool reverse, bool thru, + std::shared_ptr ec) override; chassis::DiffChassisCommand get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) override; + voss::AngularDirection direction, + std::shared_ptr ec) override; double angular_pid(double error); diff --git a/include/VOSS/controller/SwingControllerBuilder.hpp b/include/VOSS/controller/SwingControllerBuilder.hpp index 2624b98..861d92b 100644 --- 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 build(); }; diff --git a/include/VOSS/exit_conditions/AbstractExitCondition.hpp b/include/VOSS/exit_conditions/AbstractExitCondition.hpp new file mode 100644 index 0000000..3023782 --- /dev/null +++ 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 \ No newline at end of file diff --git a/include/VOSS/exit_conditions/CustomExitCondition.hpp b/include/VOSS/exit_conditions/CustomExitCondition.hpp new file mode 100644 index 0000000..d4730a0 --- /dev/null +++ b/include/VOSS/exit_conditions/CustomExitCondition.hpp @@ -0,0 +1,14 @@ +#pragma once +#include "../utils/Pose.hpp" +#include "AbstractExitCondition.hpp" +#include +namespace voss::controller { +class CustomExitCondition : public AbstractExitCondition { + private: + std::function callback; + + public: + CustomExitCondition(std::function callback); + bool is_met(Pose current_pose, bool thru) override; +}; +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/ExitConditions.hpp b/include/VOSS/exit_conditions/ExitConditions.hpp new file mode 100644 index 0000000..02a45b3 --- /dev/null +++ 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 +#include +#include + +namespace voss::controller { + +class ExitConditions : public AbstractExitCondition { + + private: + std::vector> 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 callback); + ExitConditions& add_condition(std::shared_ptr ec); + + std::shared_ptr exit_if(std::function callback); + + bool is_met(voss::Pose current_pose, bool thru); + bool all_met(voss::Pose current_pose, bool thru); + + std::shared_ptr build(); + + void reset() override; +}; + +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/PrepLineExitCondition.hpp b/include/VOSS/exit_conditions/PrepLineExitCondition.hpp new file mode 100644 index 0000000..87e6f40 --- /dev/null +++ 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 diff --git a/include/VOSS/exit_conditions/SettleExitCondition.hpp b/include/VOSS/exit_conditions/SettleExitCondition.hpp new file mode 100644 index 0000000..561ca6e --- /dev/null +++ b/include/VOSS/exit_conditions/SettleExitCondition.hpp @@ -0,0 +1,22 @@ +#pragma once + +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +namespace voss::controller { + +class SettleExitCondition : public AbstractExitCondition { + + private: + int settle_time; + int current_time; + int initial_delay; + int initial_time; + Pose prev_pose; + double tolerance; + + public: + SettleExitCondition(int settle_time, double tolerance, int initial_delay); + bool is_met(Pose current_pose, bool thru); + void reset() override; +}; +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/TimeOutExitCondition.hpp b/include/VOSS/exit_conditions/TimeOutExitCondition.hpp new file mode 100644 index 0000000..4aabe03 --- /dev/null +++ b/include/VOSS/exit_conditions/TimeOutExitCondition.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +namespace voss::controller { + +class TimeOutExitCondition : public AbstractExitCondition { + + private: + int timeout; + int current_time; + + public: + TimeOutExitCondition(int timeout); + bool is_met(Pose current_pose, bool thru); + void reset() override; +}; +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/ToleranceAngularExitCondition.hpp b/include/VOSS/exit_conditions/ToleranceAngularExitCondition.hpp new file mode 100644 index 0000000..ec57b4d --- /dev/null +++ b/include/VOSS/exit_conditions/ToleranceAngularExitCondition.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +namespace voss::controller { + +class ToleranceAngularExitCondition : public AbstractExitCondition { + + private: + double tolerance; + + public: + ToleranceAngularExitCondition(double tolerance); + bool is_met(Pose current_pose, bool thru) override; +}; +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/exit_conditions/ToleranceExitCondition.hpp b/include/VOSS/exit_conditions/ToleranceExitCondition.hpp new file mode 100644 index 0000000..bc253bf --- /dev/null +++ b/include/VOSS/exit_conditions/ToleranceExitCondition.hpp @@ -0,0 +1,18 @@ + +#pragma once +#include "AbstractExitCondition.hpp" +#include "ToleranceAngularExitCondition.hpp" +#include "ToleranceLinearExitCondition.hpp" +#include +namespace voss::controller { +class ToleranceExitCondition : public AbstractExitCondition { + private: + std::shared_ptr ang_exit = nullptr; + std::shared_ptr lin_exit = nullptr; + + public: + bool is_met(Pose pose, bool thru) override; + void add_ang_exit(double angular_tolerance); + void add_lin_exit(double linear_tolerance); +}; +} // namespace voss::controller diff --git a/include/VOSS/exit_conditions/ToleranceLinearExitCondition.hpp b/include/VOSS/exit_conditions/ToleranceLinearExitCondition.hpp new file mode 100644 index 0000000..8847afd --- /dev/null +++ b/include/VOSS/exit_conditions/ToleranceLinearExitCondition.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +namespace voss::controller { + +class ToleranceLinearExitCondition : public AbstractExitCondition { + + private: + double tolerance; + + public: + ToleranceLinearExitCondition(double tolerance); + bool is_met(Pose current_pose, bool thru) override; +}; + +} // namespace voss::controller \ No newline at end of file diff --git a/include/VOSS/utils/Pose.hpp b/include/VOSS/utils/Pose.hpp index dbbd34d..5d18743 100644 --- a/include/VOSS/utils/Pose.hpp +++ b/include/VOSS/utils/Pose.hpp @@ -1,6 +1,8 @@ #pragma once #include +#include +#include namespace voss { diff --git a/src/VOSS/chassis/AbstractChassis.cpp b/src/VOSS/chassis/AbstractChassis.cpp index a28565b..e6d6c75 100644 --- a/src/VOSS/chassis/AbstractChassis.cpp +++ b/src/VOSS/chassis/AbstractChassis.cpp @@ -1,22 +1,27 @@ #include "VOSS/chassis/AbstractChassis.hpp" #include "pros/llemu.hpp" #include "pros/rtos.hpp" +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +#include namespace voss::chassis { -AbstractChassis::AbstractChassis(controller_ptr default_controller) { +AbstractChassis::AbstractChassis(controller_ptr default_controller, ec_ptr ec) { this->default_controller = std::move(default_controller); + this->default_ec = std::move(ec); } -void AbstractChassis::move_task(controller_ptr controller, double max, - voss::Flags flags, double exitTime) { +void AbstractChassis::move_task(controller_ptr controller, ec_ptr ec, + double max, voss::Flags flags) { this->task = - std::make_unique([&, controller, flags, max, exitTime]() { + std::make_unique([&, controller, ec, flags, max]() { + ec->reset(); controller->reset(); while (!this->execute( controller->get_command(flags & voss::Flags::REVERSE, - flags & voss::Flags::THRU), + flags & voss::Flags::THRU, ec), max)) { if (pros::competition::is_disabled()) { this->task_running = false; @@ -36,17 +41,17 @@ void AbstractChassis::move_task(controller_ptr controller, double max, this->task->join(); } -void AbstractChassis::turn_task(controller_ptr controller, double max, - voss::Flags flags, - voss::AngularDirection direction, - double exitTime) { +void AbstractChassis::turn_task(controller_ptr controller, ec_ptr ec, + double max, voss::Flags flags, + voss::AngularDirection direction) { this->task = std::make_unique( - [&, controller, flags, direction, max, exitTime]() { + [&, controller, ec, flags, direction, max]() { + ec->reset(); controller->reset(); while (!this->execute(controller->get_angular_command( flags & voss::Flags::REVERSE, - flags & voss::Flags::THRU, direction), + flags & voss::Flags::THRU, direction, ec), max)) { if (pros::competition::is_disabled()) { this->task_running = false; @@ -65,62 +70,81 @@ void AbstractChassis::turn_task(controller_ptr controller, double max, this->task->join(); } -void AbstractChassis::move(Pose target, double max, voss::Flags flags, - double exitTime) { - this->move(target, this->default_controller, max, flags, exitTime); +void AbstractChassis::move(Pose target, double max, voss::Flags flags) { + this->move(target, this->default_controller, this->default_ec, max, flags); } void AbstractChassis::move(Pose target, controller_ptr controller, double max, - voss::Flags flags, double exitTime) { + voss::Flags flags) { + this->move(target, std::move(controller), this->default_ec, max, flags); +} + +void AbstractChassis::move(Pose target, controller_ptr controller, ec_ptr ec, + double max, voss::Flags flags) { while (this->task_running) { pros::delay(10); } this->task_running = true; - controller->set_target(target, flags & voss::Flags::RELATIVE); + controller->set_target(target, flags & voss::Flags::RELATIVE, ec); - this->move_task(std::move(controller), max, flags, exitTime); + this->move_task(std::move(controller), std::move(ec), max, flags); } void AbstractChassis::turn(double target, double max, voss::Flags flags, - voss::AngularDirection direction, double exitTime) { - this->turn(target, this->default_controller, max, flags, direction, - exitTime); + voss::AngularDirection direction) { + this->turn(target, this->default_controller, this->default_ec, max, flags, + direction); } void AbstractChassis::turn(double target, controller_ptr controller, double max, - voss::Flags flags, voss::AngularDirection direction, - double exitTime) { + voss::Flags flags, + voss::AngularDirection direction) { + this->turn(target, std::move(controller), this->default_ec, max, flags, + direction); +} + +void AbstractChassis::turn(double target, controller_ptr controller, ec_ptr ec, + double max, voss::Flags flags, + voss::AngularDirection direction) { while (this->task_running) { pros::delay(10); } this->task_running = true; - controller->set_target({0, 0, 0}, false); + controller->set_target({NAN, NAN, target}, flags & voss::Flags::RELATIVE, + ec); controller->set_angular_target(target, flags & voss::Flags::RELATIVE); - this->turn_task(std::move(controller), max, flags, direction, exitTime); + this->turn_task(std::move(controller), std::move(ec), max, flags, + direction); } void AbstractChassis::turn_to(Point target, double max, voss::Flags flags, - voss::AngularDirection direction, - double exitTime) { - this->turn_to(target, this->default_controller, max, flags, direction, - exitTime); + voss::AngularDirection direction) { + this->turn_to(target, this->default_controller, this->default_ec, max, + flags, direction); } void AbstractChassis::turn_to(Point target, controller_ptr controller, double max, voss::Flags flags, - voss::AngularDirection direction, - double exitTime) { + voss::AngularDirection direction) { + this->turn_to(target, std::move(controller), this->default_ec, max, flags, + direction); +} + +void AbstractChassis::turn_to(Point target, controller_ptr controller, + ec_ptr ec, double max, voss::Flags flags, + voss::AngularDirection direction) { while (this->task_running) { pros::delay(10); } this->task_running = true; controller->set_target({target.x, target.y, std::nullopt}, - flags & voss::Flags::RELATIVE); + flags & voss::Flags::RELATIVE, ec); - this->turn_task(std::move(controller), max, flags, direction, exitTime); + this->turn_task(std::move(controller), std::move(ec), max, flags, + direction); } } // namespace voss::chassis \ No newline at end of file diff --git a/src/VOSS/chassis/DiffChassis.cpp b/src/VOSS/chassis/DiffChassis.cpp index dae32c2..38f73e9 100644 --- a/src/VOSS/chassis/DiffChassis.cpp +++ b/src/VOSS/chassis/DiffChassis.cpp @@ -27,9 +27,9 @@ double DiffChassis::slew(double target, bool is_left) { // controller layouts DiffChassis::DiffChassis(std::initializer_list left_motors, std::initializer_list right_motors, - controller_ptr default_controller, double slew_step, - pros::motor_brake_mode_e brakeMode) - : AbstractChassis(default_controller) { + controller_ptr default_controller, ec_ptr ec, + double slew_step, pros::motor_brake_mode_e brakeMode) + : AbstractChassis(default_controller, ec) { this->left_motors = std::make_unique(left_motors); this->right_motors = std::make_unique(right_motors); diff --git a/src/VOSS/controller/AbstractController.cpp b/src/VOSS/controller/AbstractController.cpp index 542e673..6e3c707 100644 --- a/src/VOSS/controller/AbstractController.cpp +++ b/src/VOSS/controller/AbstractController.cpp @@ -1,6 +1,8 @@ #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" #include "VOSS/utils/angle.hpp" #include +#include namespace voss::controller { @@ -11,7 +13,8 @@ AbstractController::AbstractController( // Set desired postion with x, y, and heading // Relative target position WIP -void AbstractController::set_target(Pose target, bool relative) { +void AbstractController::set_target(Pose target, bool relative, + std::shared_ptr ec) { if (target.theta.has_value()) { target.theta = voss::to_radians(*target.theta); } @@ -28,6 +31,8 @@ void AbstractController::set_target(Pose target, bool relative) { } else { this->target = target; } + + ec->set_target(this->target); } // Set desired orientation diff --git a/src/VOSS/controller/ArcPIDController.cpp b/src/VOSS/controller/ArcPIDController.cpp index 38dbd94..060f356 100644 --- a/src/VOSS/controller/ArcPIDController.cpp +++ b/src/VOSS/controller/ArcPIDController.cpp @@ -11,8 +11,9 @@ ArcPIDController::ArcPIDController( : AbstractController(std::move(l)), prev_lin_err(0.0), total_lin_err(0.0) { } -chassis::DiffChassisCommand ArcPIDController::get_command(bool reverse, - bool thru) { +chassis::DiffChassisCommand +ArcPIDController::get_command(bool reverse, bool thru, + std::shared_ptr ec) { Point current_pos = this->l->get_position(); double current_angle = this->l->get_orientation_rad(); @@ -47,17 +48,6 @@ chassis::DiffChassisCommand ArcPIDController::get_command(bool reverse, angle_error = voss::norm_delta(angle_error); - if (distance_error <= exit_error || - (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) { - this->close += 10; - } else { - this->close = 0; - } - - if (close >= settle_time) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - double lin_speed = thru ? 100.0 : this->linear_pid(distance_error); if (distance_error < this->min_error) { @@ -88,13 +78,21 @@ chassis::DiffChassisCommand ArcPIDController::get_command(bool reverse, } prev_t = t; prev_lin_speed = lin_speed; + if (ec->is_met(this->l->get_pose(), thru)) { + if (thru) { + return chassis::DiffChassisCommand{ + chassis::diff_commands::Chained{left_speed, right_speed}}; + } else { + return chassis::Stop{}; + } + } return chassis::DiffChassisCommand{ chassis::diff_commands::Voltages{left_speed, right_speed}}; } -chassis::DiffChassisCommand -ArcPIDController::get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) { +chassis::DiffChassisCommand ArcPIDController::get_angular_command( + bool reverse, bool thru, voss::AngularDirection direction, + std::shared_ptr ec) { return chassis::DiffChassisCommand{chassis::Stop{}}; } @@ -137,17 +135,6 @@ ArcPIDController::modify_track_width(double track_width) { return this->p; } -std::shared_ptr -ArcPIDController::modify_exit_error(double exit_error) { - auto pid_mod = ArcPIDControllerBuilder::from(*this) - .with_exit_error(exit_error) - .build(); - - this->p = pid_mod; - - return this->p; -} - std::shared_ptr ArcPIDController::modify_min_error(double min_error) { auto pid_mod = @@ -158,17 +145,6 @@ ArcPIDController::modify_min_error(double min_error) { return this->p; } -std::shared_ptr -ArcPIDController::modify_settle_time(double settle_time) { - auto pid_mod = ArcPIDControllerBuilder::from(*this) - .with_settle_time(settle_time) - .build(); - - this->p = pid_mod; - - return this->p; -} - std::shared_ptr ArcPIDController::modify_slew(double slew) { auto pid_mod = ArcPIDControllerBuilder::from(*this).with_slew(slew).build(); diff --git a/src/VOSS/controller/ArcPIDControllerBuilder.cpp b/src/VOSS/controller/ArcPIDControllerBuilder.cpp index 552705a..b2d1cb2 100644 --- a/src/VOSS/controller/ArcPIDControllerBuilder.cpp +++ b/src/VOSS/controller/ArcPIDControllerBuilder.cpp @@ -39,23 +39,11 @@ ArcPIDControllerBuilder::with_track_width(double track_width) { return *this; } -ArcPIDControllerBuilder& -ArcPIDControllerBuilder::with_exit_error(double error) { - this->ctrl.exit_error = error; - return *this; -} - ArcPIDControllerBuilder& ArcPIDControllerBuilder::with_min_error(double error) { this->ctrl.min_error = error; return *this; } -ArcPIDControllerBuilder& -ArcPIDControllerBuilder::with_settle_time(double settle_time) { - this->ctrl.settle_time = settle_time; - return *this; -} - ArcPIDControllerBuilder& ArcPIDControllerBuilder::with_slew(double slew) { this->ctrl.slew = slew; return *this; diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index a2aaea1..e01627e 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -12,8 +12,9 @@ BoomerangController::BoomerangController( : AbstractController(l) { } -chassis::DiffChassisCommand BoomerangController::get_command(bool reverse, - bool thru) { +chassis::DiffChassisCommand +BoomerangController::get_command(bool reverse, bool thru, + std::shared_ptr ec) { if (!target.theta.has_value()) { return chassis::DiffChassisCommand{chassis::Stop{}}; @@ -21,91 +22,36 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse, Point current_pos = this->l->get_position(); double k = this->min_vel / 100; - Point virtualTarget = { - target.x + k * cos(target.theta.value()), - target.y + k * sin(target.theta.value())}; - double dx, dy, distance_error, at; + Point virtualTarget = {target.x + k * cos(target.theta.value()), + target.y + k * sin(target.theta.value())}; int dir = reverse ? -1 : 1; Pose trueTarget; - if (thru) { - dx = virtualTarget.x - current_pos.x; - dy = virtualTarget.y - current_pos.y; - trueTarget = {virtualTarget.x, virtualTarget.y, target.theta}; - } else { - dx = target.x - current_pos.x; - dy = target.y - current_pos.y; - trueTarget = this->target; - }; - distance_error = sqrt(dx * dx + dy * dy); - at = voss::to_radians(target.theta.value()); - - this->carrotPoint = {trueTarget.x - distance_error * cos(at) * lead_pct, - trueTarget.y - distance_error * sin(at) * lead_pct, - target.theta}; - counter += 10; + double dx = target.x - current_pos.x; + double dy = target.y - current_pos.y; + double distance_error = sqrt(dx * dx + dy * dy); + double at = voss::to_radians(target.theta.value()); + + this->carrotPoint = {this->target.x - distance_error * cos(at) * lead_pct, + this->target.y - distance_error * sin(at) * lead_pct, + target.theta}; double current_angle = this->l->get_orientation_rad() + (reverse ? M_PI : 0); bool chainedExecutable = false; bool noPose = !this->target.theta.has_value(); double angle_error; - double m = fabs((current_pos.y - target.y) / (current_pos.x - target.x)); - angle_error = atan2(dy, dx) - current_angle; angle_error = voss::norm_delta(angle_error); - double lin_speed; - - if (/*thru && - current_pos.y + this->min_error >= - (-1.0 / m) * (current_pos.x + min_error - virtualTarget.x) + - virtualTarget.y*/ - thru && - (current_pos.y - virtualTarget.y) * - cos(target.theta.value() - M_PI_2) < - (current_pos.x - virtualTarget.x) * - sin(target.theta.value() - M_PI_2) && - (pow((current_pos.y - virtualTarget.y), 2) + - pow((current_pos.x - virtualTarget.x), 2)) < - pow(this->min_error * 2, 2)) { - chainedExecutable = true; - } - - if (distance_error <= exit_error || - (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) { - total_lin_err = 0; - close += 10; - } else { - close = 0; - } - - if (close > settle_time) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - - if (fabs(distance_error - prev_lin_err) < 0.1 && - fabs(current_angle - prev_angle) < voss::to_radians(0.1) && - counter > 400) { - close_2 += 10; - } else { - close_2 = 0; - } - - prev_angle = current_angle; - - if (close_2 > settle_time * 2) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - - lin_speed = linear_pid(distance_error); + double lin_speed = linear_pid(distance_error); if (thru) { lin_speed = copysign(fmax(fabs(lin_speed), this->min_vel), lin_speed); } lin_speed *= dir; double ang_speed; - if (distance_error < min_error && !thru) { + if (distance_error < min_error) { this->can_reverse = true; if (noPose) { @@ -120,7 +66,6 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse, } // reduce the linear speed if the bot is tangent to the target - lin_speed *= cos(angle_error); } else { if (fabs(angle_error) > M_PI_2 && this->can_reverse) { @@ -132,22 +77,25 @@ chassis::DiffChassisCommand BoomerangController::get_command(bool reverse, ang_speed = angular_pid(angle_error); } - if (thru) { - lin_speed *= cos(angle_error); - } + lin_speed *= cos(angle_error); - if (chainedExecutable) { - return chassis::DiffChassisCommand{chassis::diff_commands::Chained{ - lin_speed + ang_speed, lin_speed - ang_speed}}; + if (ec->is_met(this->l->get_pose(), thru)) { + if (thru) { + return chassis::DiffChassisCommand{chassis::diff_commands::Chained{ + dir * std::fmax(lin_speed, this->min_vel) - ang_speed, + dir * std::fmax(lin_speed, this->min_vel) + ang_speed}}; + } else { + return chassis::Stop{}; + } } return chassis::DiffChassisCommand{chassis::diff_commands::Voltages{ lin_speed - ang_speed, lin_speed + ang_speed}}; } -chassis::DiffChassisCommand -BoomerangController::get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) { +chassis::DiffChassisCommand BoomerangController::get_angular_command( + bool reverse, bool thru, voss::AngularDirection direction, + std::shared_ptr ec) { return chassis::DiffChassisCommand{chassis::Stop{}}; } diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp index a44bab9..7206bf2 100644 --- a/src/VOSS/controller/PIDController.cpp +++ b/src/VOSS/controller/PIDController.cpp @@ -13,8 +13,9 @@ PIDController::PIDController(std::shared_ptr l) prev_ang_err(0.0), total_ang_err(0.0) { } -chassis::DiffChassisCommand PIDController::get_command(bool reverse, - bool thru) { +chassis::DiffChassisCommand +PIDController::get_command(bool reverse, bool thru, + std::shared_ptr ec) { // Runs in background of move commands // distance formula to find the distance between the robot and the target // position distance error is distance ArcTan is used to find the angle @@ -23,7 +24,6 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse, // Dependant on the weight of the constants and the errors // Controller exits when robot settled for a designated time duration or // accurate to a specified tolerance - counter += 10; int dir = reverse ? -1 : 1; Point current_pos = this->l->get_position(); double current_angle = this->l->get_orientation_rad(); @@ -43,42 +43,7 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse, angle_error = voss::norm_delta(angle_error); - double lin_speed; - - if (distance_error <= exit_error || - (distance_error < min_error && fabs(cos(angle_error)) <= 0.1)) { - if (thru) { - chainedExecutable = true; - } else { - total_lin_err = 0; - close += 10; - } - } else { - close = 0; - } - - if (close > settle_time) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - - if (fabs(distance_error - prev_lin_err) < 0.1 && - fabs(current_angle - prev_angle) < voss::to_radians(0.1) && - counter > 400) { - if (thru) { - chainedExecutable = true; - } - close_2 += 10; - } else { - close_2 = 0; - } - - prev_angle = current_angle; - - if (close_2 > settle_time * 2) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - - lin_speed = (thru ? 100.0 : (linear_pid(distance_error))) * dir; + double lin_speed = (thru ? 100.0 : (linear_pid(distance_error))) * dir; double ang_speed; if (distance_error < min_error) { @@ -108,10 +73,14 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse, ang_speed = angular_pid(angle_error); } // Runs at the end of a through movement - if (chainedExecutable) { - return chassis::DiffChassisCommand{chassis::diff_commands::Chained{ - dir * std::fmax(lin_speed, this->min_vel) - ang_speed, - dir * std::fmax(lin_speed, this->min_vel) + ang_speed}}; + if (ec->is_met(this->l->get_pose(), thru)) { + if (thru) { + return chassis::DiffChassisCommand{chassis::diff_commands::Chained{ + dir * std::fmax(lin_speed, this->min_vel) - ang_speed, + dir * std::fmax(lin_speed, this->min_vel) + ang_speed}}; + } else { + return chassis::Stop{}; + } } return chassis::DiffChassisCommand{chassis::diff_commands::Voltages{ @@ -120,14 +89,14 @@ chassis::DiffChassisCommand PIDController::get_command(bool reverse, chassis::DiffChassisCommand PIDController::get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) { + voss::AngularDirection direction, + std::shared_ptr ec) { // Runs in the background of turn commands // Error is the difference between the target angle and the current angle // ArcTan is used to find the angle between the robot and the target // position Output is proportional to the error and the weight of the // constant Controller exits when robot settled for a designated time // duration or accurate to a specified tolerance - counter += 10; double current_angle = this->l->get_orientation_rad(); double target_angle = 0; if (!this->target.theta.has_value()) { @@ -139,10 +108,13 @@ PIDController::get_angular_command(bool reverse, bool thru, target_angle = this->angular_target; } double angular_error = target_angle - current_angle; - + bool chainedExecutable = false; angular_error = voss::norm_delta(angular_error); if (fabs(angular_error) < voss::to_radians(5)) { + if (thru) { + chainedExecutable = true; + } turn_overshoot = true; } @@ -156,26 +128,14 @@ PIDController::get_angular_command(bool reverse, bool thru, } } - if (fabs(angular_error) < angular_exit_error) { - close += 10; - } else { - close = 0; - } - - if (close > settle_time) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - if (fabs(angular_error - prev_ang_err) < voss::to_radians(0.1) && - counter > 400) { - close_2 += 10; - } else { - close_2 = 0; - } - - if (close_2 > settle_time * 2) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } double ang_speed = angular_pid(angular_error); + if (ec->is_met(this->l->get_pose(), thru) || chainedExecutable) { + if (thru) { + return chassis::DiffChassisCommand{ + chassis::diff_commands::Chained{-ang_speed, ang_speed}}; + } + return chassis::Stop{}; + } return chassis::DiffChassisCommand{ chassis::diff_commands::Voltages{-ang_speed, ang_speed}}; } @@ -211,7 +171,6 @@ void PIDController::reset() { this->prev_ang_err = 0; this->total_ang_err = 0; this->can_reverse = false; - this->counter = 0; this->turn_overshoot = false; } @@ -246,27 +205,6 @@ std::shared_ptr PIDController::modify_tracking_kp(double kP) { return this->p; } -std::shared_ptr -PIDController::modify_exit_error(double exit_error) { - auto pid_mod = - PIDControllerBuilder::from(*this).with_exit_error(exit_error).build(); - - this->p = pid_mod; - - return this->p; -} - -std::shared_ptr -PIDController::modify_angular_exit_error(double exit_error) { - auto pid_mod = PIDControllerBuilder::from(*this) - .with_angular_exit_error(exit_error) - .build(); - - this->p = pid_mod; - - return this->p; -} - std::shared_ptr PIDController::modify_min_error(double min_error) { auto pid_mod = @@ -277,14 +215,4 @@ PIDController::modify_min_error(double min_error) { return this->p; } -std::shared_ptr -PIDController::modify_settle_time(double settle_time) { - auto pid_mod = - PIDControllerBuilder::from(*this).with_settle_time(settle_time).build(); - - this->p = pid_mod; - - return this->p; -} - } // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/controller/PIDControllerBuilder.cpp b/src/VOSS/controller/PIDControllerBuilder.cpp index 048e4b8..341a200 100644 --- a/src/VOSS/controller/PIDControllerBuilder.cpp +++ b/src/VOSS/controller/PIDControllerBuilder.cpp @@ -47,27 +47,11 @@ PIDControllerBuilder& PIDControllerBuilder::with_tracking_kp(double kP) { return *this; } -PIDControllerBuilder& PIDControllerBuilder::with_exit_error(double error) { - this->ctrl.exit_error = error; - return *this; -} - -PIDControllerBuilder& -PIDControllerBuilder::with_angular_exit_error(double error) { - this->ctrl.angular_exit_error = voss::to_radians(error); - return *this; -} - PIDControllerBuilder& PIDControllerBuilder::with_min_error(double error) { this->ctrl.min_error = error; return *this; } -PIDControllerBuilder& PIDControllerBuilder::with_settle_time(double time) { - this->ctrl.settle_time = (time > 0) ? time : 250; - return *this; -} - PIDControllerBuilder& PIDControllerBuilder::with_min_vel_for_thru(double min_vel) { this->ctrl.min_vel = min_vel; diff --git a/src/VOSS/controller/SwingController.cpp b/src/VOSS/controller/SwingController.cpp index b81beba..3e8cc15 100644 --- a/src/VOSS/controller/SwingController.cpp +++ b/src/VOSS/controller/SwingController.cpp @@ -9,14 +9,15 @@ SwingController::SwingController( std::shared_ptr l) : AbstractController(std::move(l)){}; -chassis::DiffChassisCommand SwingController::get_command(bool reverse, - bool thru) { +chassis::DiffChassisCommand +SwingController::get_command(bool reverse, bool thru, + std::shared_ptr ec) { return chassis::DiffChassisCommand{chassis::Stop{}}; } -chassis::DiffChassisCommand -SwingController::get_angular_command(bool reverse, bool thru, - voss::AngularDirection direction) { +chassis::DiffChassisCommand SwingController::get_angular_command( + bool reverse, bool thru, voss::AngularDirection direction, + std::shared_ptr ec) { // Runs in background of Swing Turn commands // ArcTan is used to find the angle between the robot and the target // position One size of the drive is locked in place while the other side @@ -25,7 +26,6 @@ SwingController::get_angular_command(bool reverse, bool thru, // Power appled to motors based on proportion of the error and the weight of // the constant Exit conditions are when the robot has settled for a // designated time duration or accurate to a specified tolerance - counter += 10; double current_angle = this->l->get_orientation_rad(); double target_angle = 0; if (!this->target.theta.has_value()) { @@ -39,8 +39,11 @@ SwingController::get_angular_command(bool reverse, bool thru, double angular_error = target_angle - current_angle; angular_error = voss::norm_delta(angular_error); - + bool chainedExecutable = false; if (fabs(angular_error) < voss::to_radians(5)) { + if (thru) { + chainedExecutable = true; + } turn_overshoot = true; } @@ -54,26 +57,6 @@ SwingController::get_angular_command(bool reverse, bool thru, } } - if (fabs(angular_error) < angular_exit_error) { - close += 10; - } else { - close = 0; - } - - if (close > settle_time) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - if (fabs(angular_error - prev_ang_err) < voss::to_radians(0.1) && - counter > 400) { - close_2 += 10; - } else { - close_2 = 0; - } - - if (close_2 > settle_time * 2) { - return chassis::DiffChassisCommand{chassis::Stop{}}; - } - double ang_speed = angular_pid(angular_error); chassis::DiffChassisCommand command; if (!((ang_speed >= 0.0) ^ (this->prev_ang_speed < 0.0)) && @@ -81,31 +64,31 @@ SwingController::get_angular_command(bool reverse, bool thru, can_reverse = !can_reverse; } - if (!this->can_reverse) { - if (!reverse) { - command = std::signbit(ang_speed) - ? chassis::diff_commands::Swing{-ang_speed, 0} - : chassis::diff_commands::Swing{0, ang_speed}; - } else { - command = std::signbit(ang_speed) - ? chassis::diff_commands::Swing{0, ang_speed} - : chassis::diff_commands::Swing{-ang_speed, 0}; - } - } else { - if (reverse) { - command = std::signbit(ang_speed) - ? chassis::diff_commands::Swing{-ang_speed, 0} - : chassis::diff_commands::Swing{0, ang_speed}; - } else { - command = std::signbit(ang_speed) - ? chassis::diff_commands::Swing{0, ang_speed} - : chassis::diff_commands::Swing{-ang_speed, 0}; + prev_ang_speed = ang_speed; + + if (ec->is_met(this->l->get_pose(), thru) || + chainedExecutable) { // exit or thru + if (thru) { + if (this->can_reverse ^ !reverse) { // same sign + return std::signbit(ang_speed) // 1 + ? chassis::diff_commands::Chained{-ang_speed, 0} + : chassis::diff_commands::Chained{0, ang_speed}; + } + return std::signbit(ang_speed) // 2 + ? chassis::diff_commands::Chained{0, ang_speed} + : chassis::diff_commands::Chained{-ang_speed, 0}; } + return chassis::Stop{}; } - prev_ang_speed = ang_speed; - - return command; + if (this->can_reverse ^ !reverse) { // same sign + return std::signbit(ang_speed) // 3 + ? chassis::diff_commands::Swing{-ang_speed, 0} + : chassis::diff_commands::Swing{0, ang_speed}; + } + return std::signbit(ang_speed) // 4 + ? chassis::diff_commands::Swing{0, ang_speed} + : chassis::diff_commands::Swing{-ang_speed, 0}; } // What is calculating the required motor power for the turn @@ -126,7 +109,6 @@ void SwingController::reset() { this->prev_ang_err = 0; this->prev_ang_speed = 0; this->total_ang_err = 0; - this->counter = 0; this->can_reverse = false; this->turn_overshoot = false; } @@ -142,26 +124,4 @@ SwingController::modify_angular_constants(double kP, double kI, double kD) { return this->p; } -std::shared_ptr -SwingController::modify_angular_exit_error(double exit_error) { - auto pid_mod = SwingControllerBuilder::from(*this) - .with_angular_exit_error(exit_error) - .build(); - - this->p = pid_mod; - - return this->p; -} - -std::shared_ptr -SwingController::modify_settle_time(double settle_time) { - auto pid_mod = SwingControllerBuilder::from(*this) - .with_settle_time(settle_time) - .build(); - - this->p = pid_mod; - - return this->p; -} - }; // namespace voss::controller diff --git a/src/VOSS/controller/SwingControllerBuilder.cpp b/src/VOSS/controller/SwingControllerBuilder.cpp index 21771d3..78d4427 100644 --- a/src/VOSS/controller/SwingControllerBuilder.cpp +++ b/src/VOSS/controller/SwingControllerBuilder.cpp @@ -35,17 +35,6 @@ SwingControllerBuilder::with_angular_constants(double kP, double kI, return *this; } -SwingControllerBuilder& -SwingControllerBuilder::with_angular_exit_error(double error) { - this->ctrl.angular_exit_error = voss::to_radians(error); - return *this; -} - -SwingControllerBuilder& SwingControllerBuilder::with_settle_time(double time) { - this->ctrl.settle_time = (time > 0) ? time : 250; - return *this; -} - std::shared_ptr SwingControllerBuilder::build() { return std::make_shared(this->ctrl); } diff --git a/src/VOSS/exit_conditions/AbstractExitCondition.cpp b/src/VOSS/exit_conditions/AbstractExitCondition.cpp new file mode 100644 index 0000000..f889548 --- /dev/null +++ b/src/VOSS/exit_conditions/AbstractExitCondition.cpp @@ -0,0 +1,19 @@ +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" + +namespace voss::controller { + +void AbstractExitCondition::set_target(Pose target) { + this->target_pose = target; +} + +void AbstractExitCondition::reset() { +} +bool AbstractExitCondition::target_has_coordinate() const { + return this->target_pose.x == this->target_pose.x && + this->target_pose.y == this->target_pose.y; +} +bool AbstractExitCondition::target_has_heading() const { + return this->target_pose.theta.has_value(); +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/CustomExitCondition.cpp b/src/VOSS/exit_conditions/CustomExitCondition.cpp new file mode 100644 index 0000000..0f0982b --- /dev/null +++ b/src/VOSS/exit_conditions/CustomExitCondition.cpp @@ -0,0 +1,13 @@ +#include "VOSS/exit_conditions/CustomExitCondition.hpp" + +#include + +namespace voss::controller { +CustomExitCondition::CustomExitCondition(std::function callback) + : callback(std::move(callback)){}; + +bool CustomExitCondition::is_met(voss::Pose current_pose, bool thru) { + return this->callback(); +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/ExitConditions.cpp b/src/VOSS/exit_conditions/ExitConditions.cpp new file mode 100644 index 0000000..5ad663e --- /dev/null +++ b/src/VOSS/exit_conditions/ExitConditions.cpp @@ -0,0 +1,110 @@ +#include "VOSS/exit_conditions/ExitConditions.hpp" +#include "VOSS/exit_conditions/CustomExitCondition.hpp" +#include "VOSS/exit_conditions/PrepLineExitCondition.hpp" +#include "VOSS/exit_conditions/SettleExitCondition.hpp" +#include "VOSS/exit_conditions/TimeOutExitCondition.hpp" +#include "VOSS/exit_conditions/ToleranceAngularExitCondition.hpp" +#include "VOSS/exit_conditions/ToleranceExitCondition.hpp" +#include "VOSS/exit_conditions/ToleranceLinearExitCondition.hpp" +#include "VOSS/utils/Pose.hpp" +#include + +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; + + for (auto ec : this->conditions) { + ec->set_target(this->target_pose); + } +} + +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; +} + +ExitConditions& ExitConditions::add_timeout(int timeout) { + this->conditions.push_back(std::make_shared(timeout)); + return *this; +} + +ExitConditions& ExitConditions::add_tolerance(double linear_tolerance, + double angular_tolerance) { + auto ec = std::make_shared(); + ec->add_lin_exit(linear_tolerance); + ec->add_ang_exit(angular_tolerance); + this->conditions.push_back( + std::dynamic_pointer_cast(ec)); + return *this; +} + +ExitConditions& ExitConditions::add_thru_smoothness(double smoothness) { + this->conditions.push_back( + std::make_shared(smoothness)); + return *this; +} + +ExitConditions& +ExitConditions::add_custom_condition(std::function callback) { + this->conditions.push_back(std::make_shared(callback)); + return *this; +} + +std::shared_ptr +ExitConditions::exit_if(std::function callback) { + std::shared_ptr ec_mod = + std::make_shared(*this); + ec_mod->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; +} + +bool ExitConditions::is_met(voss::Pose current_pose, bool thru) { + for (auto ec : this->conditions) { + if (ec->is_met(current_pose, thru)) { + return true; + } + } + + return false; +} + +bool ExitConditions::all_met(voss::Pose current_pose, bool thru) { + for (auto ec : this->conditions) { + if (!ec->is_met(current_pose, thru)) { + return false; + } + } + + return true; +} + +std::shared_ptr ExitConditions::build() { + return std::make_shared(*this); +} + +void ExitConditions::reset() { + for (auto ec : this->conditions) { + ec->reset(); + } +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/PrepLineExitCondition.cpp b/src/VOSS/exit_conditions/PrepLineExitCondition.cpp new file mode 100644 index 0000000..2d10102 --- /dev/null +++ b/src/VOSS/exit_conditions/PrepLineExitCondition.cpp @@ -0,0 +1,48 @@ +#include "VOSS/exit_conditions/PrepLineExitCondition.hpp" +#include "VOSS/utils/Point.hpp" +#include + +namespace voss::controller { +PrepLineExitCondition::PrepLineExitCondition(double thru_smoothness) + : thru_smoothness(thru_smoothness) { +} + +bool PrepLineExitCondition::is_met(voss::Pose pose, bool thru) { + if (thru) { + if (this->target_pose.theta.has_value()) { // semi circle exit + bool exit = (pose.y - this->target_pose.y) * + cos(target_pose.theta.value() - M_PI_2) < + (pose.x - target_pose.x) * + sin(target_pose.theta.value() - M_PI_2) && + (pow((pose.y - target_pose.y), 2) + + pow((pose.x - target_pose.x), 2)) < + pow(this->thru_smoothness * 2, 2); + // if (exit) { + // printf("Prep line cond met 1\n"); + // } + return exit; + } else { // line exit + // + // /*thru && + // current_pos.y + this->min_error >= + // (-1.0 / m) * (current_pos.x + min_error - + // virtualTarget.x) + + // virtualTarget.y*/ + // double m = fabs((pose.y - this->target_pose.y) + // / (pose.x - this->target_pose.x)); + // bool exit = pose.y + this->thru_smoothness >= + // (-1.0 / m) * (pose.x + thru_smoothness - + // target_pose.x) + + // target_pose.y; + double d = voss::Point::getDistance( + {this->target_pose.x, this->target_pose.y}, {pose.x, pose.y}); + bool exit = d < this->thru_smoothness; + // if (exit) { + // printf("Prep line cond met 2\n"); + // } + return exit; + } + } + return false; +} +}; // namespace voss::controller diff --git a/src/VOSS/exit_conditions/SettleExitCondition.cpp b/src/VOSS/exit_conditions/SettleExitCondition.cpp new file mode 100644 index 0000000..ba53118 --- /dev/null +++ b/src/VOSS/exit_conditions/SettleExitCondition.cpp @@ -0,0 +1,56 @@ +#include "VOSS/exit_conditions/SettleExitCondition.hpp" +#include "VOSS/exit_conditions/AbstractExitCondition.hpp" +#include "VOSS/utils/angle.hpp" +#include "VOSS/utils/Pose.hpp" +#include +#include + +namespace voss::controller { +bool SettleExitCondition::is_met(Pose current_pose, bool thru) { + if (initial_time <= initial_delay) { + initial_time += 10; + return false; + } + if (this->current_time < this->settle_time) { + if (std::abs(current_pose.x - this->prev_pose.x) < this->tolerance && + std::abs(current_pose.y - this->prev_pose.y) < this->tolerance) { + if (current_pose.theta.has_value()) { + if (std::abs(current_pose.theta.value() - + this->prev_pose.theta.value()) < + voss::to_radians(this->tolerance)) { + this->current_time += 10; + } + } + + // if(thru) { + // printf("settle condition met\n"); + // return true; + // } + + } else { + current_time = 0; + prev_pose = current_pose; + } + } else { + // printf("settle condition met\n"); + return true; + } + return false; +} + +SettleExitCondition::SettleExitCondition(int settle_time, double tolerance, + int initial_delay) + : settle_time(settle_time), tolerance(tolerance), + initial_delay(initial_delay) { + this->current_time = 0; + this->initial_time = 0; + this->prev_pose = Pose{0, 0, 0}; +} + +void SettleExitCondition::reset() { + this->current_time = 0; + this->initial_time = 0; + this->prev_pose = Pose{0, 0, 0}; +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/TimeOutExitCondition.cpp b/src/VOSS/exit_conditions/TimeOutExitCondition.cpp new file mode 100644 index 0000000..57c3e90 --- /dev/null +++ b/src/VOSS/exit_conditions/TimeOutExitCondition.cpp @@ -0,0 +1,21 @@ +#include "VOSS/exit_conditions/TimeOutExitCondition.hpp" +#include + +namespace voss::controller { + +TimeOutExitCondition::TimeOutExitCondition(int timeout) : timeout(timeout) { + this->current_time = 0; +} + +bool TimeOutExitCondition::is_met(Pose current_pose, bool thru) { + this->current_time += 10; + // if (this->current_time >= this->timeout) + // printf("Time out cond met\n"); + return this->current_time >= this->timeout; +} + +void TimeOutExitCondition::reset() { + this->current_time = 0; +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp new file mode 100644 index 0000000..1319165 --- /dev/null +++ b/src/VOSS/exit_conditions/ToleranceAngularExitCondition.cpp @@ -0,0 +1,21 @@ +#include "VOSS/exit_conditions/ToleranceAngularExitCondition.hpp" +#include "VOSS/utils/angle.hpp" +#include + +namespace voss::controller { + +ToleranceAngularExitCondition::ToleranceAngularExitCondition(double tolerance) + : tolerance(tolerance) { +} + +bool ToleranceAngularExitCondition::is_met(Pose current_pose, bool thru) { + // if there is no target angle, return false so the other conditions run. + if (!this->target_pose.theta.has_value()) { + return false; + } + return std::abs(voss::norm_delta(current_pose.theta.value() - + this->target_pose.theta.value())) < + this->tolerance; +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp new file mode 100644 index 0000000..9b2a67f --- /dev/null +++ b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp @@ -0,0 +1,25 @@ + +#include "VOSS/exit_conditions/ToleranceExitCondition.hpp" + +namespace voss::controller { + +bool ToleranceExitCondition::is_met(Pose pose, bool thru) { + if (this->target_has_coordinate() && this->target_has_heading()) { + return ang_exit->is_met(pose, thru) && ang_exit->is_met(pose, thru); + } else if (this->target_has_coordinate()) { + return lin_exit->is_met(pose, thru); + } else if (this->target_has_heading()) { + return ang_exit->is_met(pose, thru); + } + return false; +} +void ToleranceExitCondition::add_ang_exit(double angular_tolerance) { + this->ang_exit = + std::make_shared(angular_tolerance); +} + +void ToleranceExitCondition::add_lin_exit(double linear_tolerance) { + this->lin_exit = + std::make_shared(linear_tolerance); +} +} // namespace voss::controller diff --git a/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp new file mode 100644 index 0000000..44e8b5f --- /dev/null +++ b/src/VOSS/exit_conditions/ToleranceLinearExitCondition.cpp @@ -0,0 +1,22 @@ +#include "VOSS/exit_conditions/ToleranceLinearExitCondition.hpp" +#include "VOSS/utils/Point.hpp" +#include +#include + +namespace voss::controller { + +ToleranceLinearExitCondition::ToleranceLinearExitCondition(double tolerance) + : tolerance(tolerance) { +} + +bool ToleranceLinearExitCondition::is_met(Pose current_pose, bool thru) { + // printf("current pose: %f %f\n", current_pose.x, current_pose.y); + // printf("target pose: %f %f\n", target_pose.x, target_pose.y); + double d = + voss::Point::getDistance({this->target_pose.x, this->target_pose.y}, + {current_pose.x, current_pose.y}); + bool met = d < this->tolerance; + return met; +} + +} // namespace voss::controller \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 95ffaa6..31f511a 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,10 +22,7 @@ auto odom = voss::localizer::IMELocalizerBuilder::new_builder() auto pid = voss::controller::PIDControllerBuilder::new_builder(odom) .with_linear_constants(20, 0.02, 169) .with_angular_constants(250, 0.05, 2435) - .with_exit_error(1.0) - .with_angular_exit_error(2.0) .with_min_error(5) - .with_settle_time(200) .with_min_vel_for_thru(100) .build(); @@ -42,21 +39,29 @@ auto boomerang = voss::controller::BoomerangControllerBuilder::new_builder(odom) auto swing = voss::controller::SwingControllerBuilder::new_builder(odom) .with_angular_constants(250, 0.05, 2435) - .with_angular_exit_error(0.5) - .with_settle_time(200) .build(); auto arc = voss::controller::ArcPIDControllerBuilder(odom) .with_track_width(14) .with_linear_constants(20, 0.02, 169) - .with_exit_error(1.0) .with_min_error(5) - .with_settle_time(200) .build(); -auto chassis = voss::chassis::DiffChassis(LEFT_MOTORS, RIGHT_MOTORS, pid, 0, +pros::Controller master(pros::E_CONTROLLER_MASTER); +auto ec = voss::controller::ExitConditions::new_conditions() + .add_settle(400, 0.5, 400) + .add_tolerance(1.0, 2.0) + .add_timeout(22500) + .add_thru_smoothness(4) + .build() -> exit_if([]() { + return master.get_digital(pros::E_CONTROLLER_DIGITAL_UP); + }); + +auto chassis = voss::chassis::DiffChassis(LEFT_MOTORS, RIGHT_MOTORS, pid, ec, pros::E_MOTOR_BRAKE_COAST); +pros::IMU imu(16); + /** * Runs initialization code. This occurs as soon as the program is started. * @@ -128,7 +133,6 @@ void autonomous() { * task, not resume it from where it left off. */ void opcontrol() { - pros::Controller master(pros::E_CONTROLLER_MASTER); while (true) { voss::Pose p = odom->get_pose(); @@ -137,13 +141,25 @@ void opcontrol() { master.get_analog(ANALOG_RIGHT_X)); if (master.get_digital_new_press(DIGITAL_Y)) { - odom->set_pose({0.0, 0.0, 90}); - chassis.move({0, 48}); - chassis.turn_to({0, 0}); - // chassis.turn(270); - chassis.move({10, 10, 250}, boomerang, 50); - odom->set_pose({20, 10}); - pros::delay(2000); + odom->set_pose({0.0, 0.0, 270}); + chassis.move({24, 24, 45}, boomerang, 100, + voss::Flags::THRU | voss::Flags::REVERSE); + printf("1.\n"); + master.rumble("--"); + chassis.turn(90, 100, voss::Flags::THRU); + printf("2.\n"); + master.rumble("--"); + chassis.move({-10, 60, 180}, boomerang, 100, voss::Flags::THRU); + printf("3.\n"); + master.rumble("--"); + chassis.turn(270, swing, 100, + voss::Flags::REVERSE | voss::Flags::THRU); + printf("4.\n"); + master.rumble("--"); + chassis.move({10, 30}, 100, voss::Flags::THRU); + printf("5.\n"); + master.rumble("--"); + chassis.turn(0); } pros::lcd::clear_line(1);