diff --git a/include/VOSS/chassis/AbstractChassis.hpp b/include/VOSS/chassis/AbstractChassis.hpp index 7476f5f6..9dc702cd 100644 --- a/include/VOSS/chassis/AbstractChassis.hpp +++ b/include/VOSS/chassis/AbstractChassis.hpp @@ -24,7 +24,7 @@ class AbstractChassis { double exitTime); void turn_task(controller_ptr controller, double max, voss::Flags flags, - double exitTime); + voss::AngularDirection direction, double exitTime); public: AbstractChassis(controller_ptr default_controller); @@ -44,15 +44,23 @@ class AbstractChassis { voss::Flags flags = voss::Flags::NONE, double exitTime = 22500); void turn(double target, controller_ptr controller, double max = 100.0, - voss::Flags flags = voss::Flags::NONE, double exitTime = 22500); + voss::Flags flags = voss::Flags::NONE, + voss::AngularDirection direction = voss::AngularDirection::AUTO, + double exitTime = 22500); void turn(double target, double max = 100.0, - voss::Flags flags = voss::Flags::NONE, double exitTime = 22500); - void turn_to(Point target, controller_ptr controller, double max = 100.0, - voss::Flags flags = voss::Flags::NONE, - double exitTime = 22500); - void turn_to(Point target, double max = 100.0, - voss::Flags flags = voss::Flags::NONE, - double exitTime = 22500); + voss::Flags flags = voss::Flags::NONE, + voss::AngularDirection direction = voss::AngularDirection::AUTO, + double exitTime = 22500); + 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); + void + turn_to(Point target, double max = 100.0, + voss::Flags flags = voss::Flags::NONE, + voss::AngularDirection direction = voss::AngularDirection::AUTO, + double exitTime = 22500); }; } // namespace voss::chassis \ No newline at end of file diff --git a/include/VOSS/controller/AbstractController.hpp b/include/VOSS/controller/AbstractController.hpp index a7e38de1..ead64130 100644 --- a/include/VOSS/controller/AbstractController.hpp +++ b/include/VOSS/controller/AbstractController.hpp @@ -2,6 +2,7 @@ #include "VOSS/chassis/ChassisCommand.hpp" #include "VOSS/localizer/AbstractLocalizer.hpp" +#include "VOSS/utils/flags.hpp" namespace voss::controller { @@ -16,8 +17,9 @@ class AbstractController { AbstractController(std::shared_ptr l); virtual chassis::ChassisCommand get_command(bool reverse, bool thru) = 0; - virtual chassis::ChassisCommand get_angular_command(bool reverse, - bool thru) = 0; + virtual chassis::ChassisCommand + get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) = 0; virtual void reset() = 0; diff --git a/include/VOSS/controller/ArcPIDController.hpp b/include/VOSS/controller/ArcPIDController.hpp index 617310ed..0925bf00 100644 --- a/include/VOSS/controller/ArcPIDController.hpp +++ b/include/VOSS/controller/ArcPIDController.hpp @@ -26,8 +26,9 @@ class ArcPIDController : public AbstractController { double linear_pid(double error); chassis::ChassisCommand get_command(bool reverse, bool thru) override; - chassis::ChassisCommand get_angular_command(bool reverse, - bool thru) override; + chassis::ChassisCommand + get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) override; void reset() override; diff --git a/include/VOSS/controller/BoomerangController.hpp b/include/VOSS/controller/BoomerangController.hpp index 4d6d9abd..fdc5c074 100644 --- a/include/VOSS/controller/BoomerangController.hpp +++ b/include/VOSS/controller/BoomerangController.hpp @@ -4,6 +4,7 @@ #include "PIDController.hpp" #include "VOSS/chassis/ChassisCommand.hpp" #include "VOSS/localizer/AbstractLocalizer.hpp" +#include "VOSS/utils/flags.hpp" namespace voss::controller { @@ -16,8 +17,9 @@ class BoomerangController : public AbstractController { BoomerangController(std::shared_ptr l); chassis::ChassisCommand get_command(bool reverse, bool thru) override; - chassis::ChassisCommand get_angular_command(bool reverse, - bool thru) override; + chassis::ChassisCommand + get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) override; void reset(); diff --git a/include/VOSS/controller/PIDController.hpp b/include/VOSS/controller/PIDController.hpp index d6c2757f..772e8187 100644 --- a/include/VOSS/controller/PIDController.hpp +++ b/include/VOSS/controller/PIDController.hpp @@ -1,6 +1,7 @@ #pragma once #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/utils/flags.hpp" namespace voss::controller { @@ -19,6 +20,7 @@ class PIDController : public AbstractController { double close_2; int counter; double prev_angle; + bool turn_overshoot; double prev_lin_err, total_lin_err, prev_ang_err, total_ang_err; @@ -29,8 +31,9 @@ class PIDController : public AbstractController { double angular_pid(double error); chassis::ChassisCommand get_command(bool reverse, bool thru) override; - chassis::ChassisCommand get_angular_command(bool reverse, - bool thru) override; + chassis::ChassisCommand + get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) override; void reset() override; diff --git a/include/VOSS/controller/SwingController.hpp b/include/VOSS/controller/SwingController.hpp index c6f24825..9eac0121 100644 --- a/include/VOSS/controller/SwingController.hpp +++ b/include/VOSS/controller/SwingController.hpp @@ -2,6 +2,7 @@ #include "PIDController.hpp" #include "VOSS/controller/AbstractController.hpp" +#include "VOSS/utils/flags.hpp" namespace voss::controller { class SwingController : public AbstractController { @@ -14,6 +15,7 @@ class SwingController : public AbstractController { double close; double close_2; int counter; + bool turn_overshoot; double prev_ang_err, total_ang_err; double prev_ang_speed; @@ -22,8 +24,9 @@ class SwingController : public AbstractController { SwingController(std::shared_ptr l); chassis::ChassisCommand get_command(bool reverse, bool thru) override; - chassis::ChassisCommand get_angular_command(bool reverse, - bool thru) override; + chassis::ChassisCommand + get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) override; double angular_pid(double error); diff --git a/include/VOSS/utils/angle.hpp b/include/VOSS/utils/angle.hpp index ebedffd4..6b9f14d5 100644 --- a/include/VOSS/utils/angle.hpp +++ b/include/VOSS/utils/angle.hpp @@ -1,13 +1,27 @@ #pragma once +#include "VOSS/utils/flags.hpp" +#include + namespace voss { -double to_radians(double degrees); +inline double to_radians(double degrees) { + return degrees * M_PI / 180; +} -double to_degrees(double radians); +inline double to_degrees(double radians) { + return radians * 180 * M_1_PI; +} -double norm(double radians); +inline double norm(double radians) { + radians = fmod(radians, 2 * M_PI); + if (radians < 0) + radians += 2 * M_PI; + return radians; +} -double norm_delta(double radians); +inline double norm_delta(double radians) { + return std::remainder(radians, 2 * M_PI); +} } // namespace voss \ No newline at end of file diff --git a/include/VOSS/utils/flags.hpp b/include/VOSS/utils/flags.hpp index 1a1f83c1..559ba90f 100644 --- a/include/VOSS/utils/flags.hpp +++ b/include/VOSS/utils/flags.hpp @@ -21,4 +21,6 @@ auto inline operator&(Flags flag1, Flags flag2) { return static_cast(static_cast(flag1) & static_cast(flag2)); } + +enum class AngularDirection { AUTO, COUNTERCLOCKWISE, CLOCKWISE }; } // namespace voss \ No newline at end of file diff --git a/src/VOSS/chassis/AbstractChassis.cpp b/src/VOSS/chassis/AbstractChassis.cpp index 2092e781..52c83a06 100644 --- a/src/VOSS/chassis/AbstractChassis.cpp +++ b/src/VOSS/chassis/AbstractChassis.cpp @@ -41,14 +41,16 @@ void AbstractChassis::move_task(controller_ptr controller, double max, } void AbstractChassis::turn_task(controller_ptr controller, double max, - voss::Flags flags, double exitTime) { + voss::Flags flags, + voss::AngularDirection direction, + double exitTime) { int t = 0; pros::Task running_t([&, controller]() { controller->reset(); - while (!this->execute( - controller->get_angular_command(flags & voss::Flags::REVERSE, - flags & voss::Flags::THRU), - max)) { + while (!this->execute(controller->get_angular_command( + flags & voss::Flags::REVERSE, + flags & voss::Flags::THRU, direction), + max)) { if (pros::competition::is_disabled()) { return; } @@ -95,33 +97,39 @@ void AbstractChassis::move(Pose target, controller_ptr controller, double max, } void AbstractChassis::turn(double target, double max, voss::Flags flags, - double exitTime) { - this->turn(target, this->default_controller, max, flags, exitTime); + voss::AngularDirection direction, double exitTime) { + this->turn(target, this->default_controller, max, flags, direction, + exitTime); } void AbstractChassis::turn(double target, controller_ptr controller, double max, - voss::Flags flags, double exitTime) { + voss::Flags flags, voss::AngularDirection direction, + double exitTime) { // this->m.take(); controller->set_target({0, 0, 0}, false); controller->set_angular_target(target, flags & voss::Flags::RELATIVE); - this->turn_task(controller, max, flags, exitTime); + this->turn_task(controller, max, flags, direction, exitTime); } 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, exitTime); + this->turn_to(target, this->default_controller, max, flags, direction, + exitTime); } void AbstractChassis::turn_to(Point target, controller_ptr controller, - double max, voss::Flags flags, double exitTime) { + double max, voss::Flags flags, + voss::AngularDirection direction, + double exitTime) { // this->m.take(); controller->set_target({target.x, target.y, 361}, flags & voss::Flags::RELATIVE); - this->turn_task(controller, max, flags, exitTime); + this->turn_task(controller, max, flags, direction, exitTime); } } // namespace voss::chassis \ No newline at end of file diff --git a/src/VOSS/controller/ArcPIDController.cpp b/src/VOSS/controller/ArcPIDController.cpp index f9170e94..a893c00c 100644 --- a/src/VOSS/controller/ArcPIDController.cpp +++ b/src/VOSS/controller/ArcPIDController.cpp @@ -88,8 +88,9 @@ chassis::ChassisCommand ArcPIDController::get_command(bool reverse, bool thru) { return chassis::ChassisCommand{chassis::Voltages{left_speed, right_speed}}; } -chassis::ChassisCommand ArcPIDController::get_angular_command(bool reverse, - bool thru) { +chassis::ChassisCommand +ArcPIDController::get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) { return chassis::ChassisCommand{chassis::Stop{}}; } diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index b39a7272..f47ffa05 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -32,11 +32,12 @@ chassis::ChassisCommand BoomerangController::get_command(bool reverse, return child->get_command(reverse, thru); } -chassis::ChassisCommand BoomerangController::get_angular_command(bool reverse, - bool thru) { +chassis::ChassisCommand +BoomerangController::get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) { child->set_target(target, false); child->set_angular_target(angular_target, false); - return child->get_angular_command(reverse, thru); + return child->get_angular_command(reverse, thru, direction); } void BoomerangController::reset() { diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp index fc792bb3..0c75662b 100644 --- a/src/VOSS/controller/PIDController.cpp +++ b/src/VOSS/controller/PIDController.cpp @@ -104,8 +104,9 @@ chassis::ChassisCommand PIDController::get_command(bool reverse, bool thru) { chassis::Voltages{lin_speed - ang_speed, lin_speed + ang_speed}}; } -chassis::ChassisCommand PIDController::get_angular_command(bool reverse, - bool thru) { +chassis::ChassisCommand +PIDController::get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) { counter += 10; double current_angle = this->l->get_orientation_rad(); double target_angle = 0; @@ -121,6 +122,20 @@ chassis::ChassisCommand PIDController::get_angular_command(bool reverse, angular_error = voss::norm_delta(angular_error); + if (fabs(angular_error) < voss::to_radians(5)) { + turn_overshoot = true; + } + + if (!turn_overshoot) { + if (direction == voss::AngularDirection::COUNTERCLOCKWISE && + angular_error < 0) { + angular_error += 2 * M_PI; + } else if (direction == voss::AngularDirection::CLOCKWISE && + angular_error > 0) { + angular_error -= 2 * M_PI; + } + } + if (fabs(angular_error) < angular_exit_error) { close += 10; } else { @@ -173,6 +188,7 @@ void PIDController::reset() { this->total_ang_err = 0; this->can_reverse = false; this->counter = 0; + this->turn_overshoot = false; } } // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/controller/SwingController.cpp b/src/VOSS/controller/SwingController.cpp index 40118cc4..a49eecf9 100644 --- a/src/VOSS/controller/SwingController.cpp +++ b/src/VOSS/controller/SwingController.cpp @@ -10,8 +10,9 @@ chassis::ChassisCommand SwingController::get_command(bool reverse, bool thru) { return chassis::ChassisCommand{chassis::Stop{}}; } -chassis::ChassisCommand SwingController::get_angular_command(bool reverse, - bool thru) { +chassis::ChassisCommand +SwingController::get_angular_command(bool reverse, bool thru, + voss::AngularDirection direction) { counter += 10; double current_angle = this->l->get_orientation_rad(); double target_angle = 0; @@ -27,6 +28,20 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse, angular_error = voss::norm_delta(angular_error); + if (fabs(angular_error) < voss::to_radians(5)) { + turn_overshoot = true; + } + + if (!turn_overshoot) { + if (direction == voss::AngularDirection::COUNTERCLOCKWISE && + angular_error < 0) { + angular_error += 2 * M_PI; + } else if (direction == voss::AngularDirection::CLOCKWISE && + angular_error > 0) { + angular_error -= 2 * M_PI; + } + } + if (fabs(angular_error) < angular_exit_error) { close += 10; } else { @@ -51,7 +66,7 @@ chassis::ChassisCommand SwingController::get_angular_command(bool reverse, chassis::ChassisCommand command; if (!((ang_speed >= 0.0) ^ (this->prev_ang_speed < 0.0)) && this->prev_ang_speed != 0) { - can_reverse = true; + can_reverse = !can_reverse; } if (!this->can_reverse) { @@ -94,5 +109,6 @@ void SwingController::reset() { this->total_ang_err = 0; this->counter = 0; this->can_reverse = false; + this->turn_overshoot = false; } }; // namespace voss::controller diff --git a/src/VOSS/utils/angle.cpp b/src/VOSS/utils/angle.cpp deleted file mode 100644 index e2d44f0e..00000000 --- a/src/VOSS/utils/angle.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "VOSS/utils/angle.hpp" - -#include - -namespace voss { - -double to_radians(double degrees) { - return degrees * M_PI / 180; -} - -double to_degrees(double radians) { - return radians * 180 * M_1_PI; -} - -double norm(double radians) { - radians = fmod(radians, 2 * M_PI); - if (radians < 0) - radians += 2 * M_PI; - return radians; -} - -double norm_delta(double radians) { - return std::remainder(radians, 2 * M_PI); -} -} // namespace voss \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 28f0ca67..7c90c446 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -107,8 +107,11 @@ void opcontrol() { if (master.get_digital_new_press(DIGITAL_Y)) { odom->set_pose(voss::Pose{0.0, 0.0, 0}); - chassis.turn(180, 100, voss::Flags::NONE, 10000); - chassis.turn(0, swing); + chassis.turn(270, 100, voss::Flags::NONE, + voss::AngularDirection::COUNTERCLOCKWISE); + chassis.turn(0); + chassis.turn(-270, swing, 100, voss::Flags::NONE, + voss::AngularDirection::CLOCKWISE); } pros::lcd::clear_line(1);