From 94d99e771f7660ec4e2dcdaa7ea8da90fd1dca25 Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Mon, 31 Mar 2025 15:38:58 -0400 Subject: [PATCH 1/4] improvements --- include/VOSS/api.hpp | 1 + include/VOSS/chassis/AbstractChassis.hpp | 3 + .../VOSS/controller/BoomerangController.hpp | 24 +++++- include/VOSS/controller/PIDController.hpp | 25 ++++++ include/VOSS/localizer/AbstractLocalizer.hpp | 2 + .../VOSS/localizer/TrackingWheelLocalizer.hpp | 3 + include/VOSS/utils/debug.hpp | 9 ++ src/VOSS/chassis/AbstractChassis.cpp | 12 ++- src/VOSS/controller/BoomerangController.cpp | 81 +++++++++++++----- src/VOSS/controller/PIDController.cpp | 83 +++++++++++++++---- .../exit_conditions/PrepLineExitCondition.cpp | 39 ++------- .../exit_conditions/SettleExitCondition.cpp | 5 +- .../exit_conditions/TimeOutExitCondition.cpp | 7 +- .../ToleranceExitCondition.cpp | 13 ++- src/VOSS/localizer/AbstractLocalizer.cpp | 25 +++++- src/VOSS/localizer/TrackingWheelLocalizer.cpp | 32 ++++--- src/VOSS/utils/debug.cpp | 18 ++++ src/main.cpp | 36 ++++---- 18 files changed, 310 insertions(+), 108 deletions(-) create mode 100644 include/VOSS/utils/debug.hpp create mode 100644 src/VOSS/utils/debug.cpp diff --git a/include/VOSS/api.hpp b/include/VOSS/api.hpp index 7fc5501a..da4922bc 100644 --- a/include/VOSS/api.hpp +++ b/include/VOSS/api.hpp @@ -35,6 +35,7 @@ #include "selector/Selector.hpp" #include "utils/angle.hpp" +#include "utils/debug.hpp" #include "utils/flags.hpp" #include "utils/Point.hpp" #include "utils/Pose.hpp" \ No newline at end of file diff --git a/include/VOSS/chassis/AbstractChassis.hpp b/include/VOSS/chassis/AbstractChassis.hpp index 22c43818..27dad891 100644 --- a/include/VOSS/chassis/AbstractChassis.hpp +++ b/include/VOSS/chassis/AbstractChassis.hpp @@ -39,6 +39,9 @@ class AbstractChassis { virtual bool execute(DiffChassisCommand cmd, double max) = 0; virtual void set_brake_mode(pros::motor_brake_mode_e mode) = 0; + void wait_until_done() const; + bool is_running() const; + void move(double distance, double max = 100.0, voss::Flags flags = voss::Flags::NONE); diff --git a/include/VOSS/controller/BoomerangController.hpp b/include/VOSS/controller/BoomerangController.hpp index 72990f48..b9d9cc28 100644 --- a/include/VOSS/controller/BoomerangController.hpp +++ b/include/VOSS/controller/BoomerangController.hpp @@ -11,10 +11,22 @@ namespace voss::controller { class BoomerangController : public AbstractController { + public: + enum class ThruBehavior { // affects calculation of lin_speed on thru movements + MIN_VEL, // use linear PID, but enforce a minimum velocity + FULL_SPEED // no PID, go full speed + }; + enum class CosineScaling { + MIN_ERR, // only do cosine scaling within the minimum err + ALL_THE_TIME // do cosine scaling all the time + }; + enum class MinErrBehavior { + TARGET_HEADING, // directly switch from turning towards carrot point to turning towards target heading + SCALE_TARGET_HEADING, // scale between turning towards carrot point to turning towards target point + }; protected: std::shared_ptr p; double lead_pct; - Pose carrotPoint; utils::PID linear_pid, angular_pid; double min_error; @@ -22,6 +34,11 @@ class BoomerangController : public AbstractController { double min_vel; + ThruBehavior thru_behavior = ThruBehavior::FULL_SPEED; + CosineScaling cosine_scaling = CosineScaling::ALL_THE_TIME; + MinErrBehavior min_err_behavior = MinErrBehavior::SCALE_TARGET_HEADING; + bool enable_overturn = false; + public: BoomerangController(std::shared_ptr l); @@ -42,6 +59,11 @@ class BoomerangController : public AbstractController { std::shared_ptr modify_min_error(double error); std::shared_ptr modify_lead_pct(double lead_pct); + void set_thru_behavior(ThruBehavior thru_behavior); + void set_cosine_scaling(CosineScaling cosine_scaling); + void set_min_err_behavior(MinErrBehavior min_err_behavior); + void set_overturn(bool overturn); + friend class BoomerangControllerBuilder; }; diff --git a/include/VOSS/controller/PIDController.hpp b/include/VOSS/controller/PIDController.hpp index c0c90a6f..16a80237 100644 --- a/include/VOSS/controller/PIDController.hpp +++ b/include/VOSS/controller/PIDController.hpp @@ -7,16 +7,36 @@ namespace voss::controller { class PIDController : public AbstractController { + public: + enum class ThruBehavior { // affects calculation of lin_speed on thru movements + MIN_VEL, // use linear PID, but enforce a minimum velocity + FULL_SPEED // no PID, go full speed + }; + enum class CosineScaling { + MIN_ERR, // only do cosine scaling within the minimum err + ALL_THE_TIME // do cosine scaling all the time + }; + enum class MinErrBehavior { + NO_ANG_PID, // no angular PID, set ang_speed to 0 + SCALE_ANG_PID, // scale output of angular pid so it reaches 0 at half of min_err + MAINTAIN_HEADING // maintain heading of when it entered min_err + }; protected: std::shared_ptr p; utils::PID linear_pid, angular_pid; double min_error; bool can_reverse; + double min_err_heading; double min_vel; bool turn_overshoot; + ThruBehavior thru_behavior = ThruBehavior::FULL_SPEED; + CosineScaling cosine_scaling = CosineScaling::MIN_ERR; + MinErrBehavior min_err_behavior = MinErrBehavior::MAINTAIN_HEADING; + bool enable_overturn = false; + public: PIDController(std::shared_ptr l); @@ -36,6 +56,11 @@ class PIDController : public AbstractController { modify_angular_constants(double kP, double kI, double kD); std::shared_ptr modify_min_error(double min_error); + void set_thru_behavior(ThruBehavior thru_behavior); + void set_cosine_scaling(CosineScaling cosine_scaling); + void set_min_err_behavior(MinErrBehavior min_err_behavior); + void set_overturn(bool overturn); + friend class PIDControllerBuilder; friend class BoomerangControllerBuilder; }; diff --git a/include/VOSS/localizer/AbstractLocalizer.hpp b/include/VOSS/localizer/AbstractLocalizer.hpp index 0e2c3ed7..c9ba566b 100644 --- a/include/VOSS/localizer/AbstractLocalizer.hpp +++ b/include/VOSS/localizer/AbstractLocalizer.hpp @@ -27,6 +27,8 @@ class AbstractLocalizer { double get_orientation_rad(); double get_orientation_deg(); Point get_position(); + void wait_until_near(Point target, double tolerance); + void wait_until_distance(double distance); virtual void calibrate() = 0; }; diff --git a/include/VOSS/localizer/TrackingWheelLocalizer.hpp b/include/VOSS/localizer/TrackingWheelLocalizer.hpp index dba9bb31..f50710a2 100644 --- a/include/VOSS/localizer/TrackingWheelLocalizer.hpp +++ b/include/VOSS/localizer/TrackingWheelLocalizer.hpp @@ -12,6 +12,8 @@ class TrackingWheelLocalizer : public AbstractLocalizer { protected: std::atomic prev_left_pos, prev_right_pos, prev_middle_pos; AtomicPose prev_pose; + AtomicPose real_pose; + Pose local_offset = {0, 0, 0}; std::atomic left_right_dist, middle_dist; std::unique_ptr left_tracking_wheel, @@ -28,6 +30,7 @@ class TrackingWheelLocalizer : public AbstractLocalizer { void calibrate() override; void set_pose(Pose pose) override; void set_pose(double x, double y, double theta) override; + void set_local_offset(Pose local_offset); friend class TrackingWheelLocalizerBuilder; }; diff --git a/include/VOSS/utils/debug.hpp b/include/VOSS/utils/debug.hpp new file mode 100644 index 00000000..c810c608 --- /dev/null +++ b/include/VOSS/utils/debug.hpp @@ -0,0 +1,9 @@ +#pragma once + +namespace voss { + +void enable_debug(); +void disable_debug(); +bool get_debug(); + +} \ No newline at end of file diff --git a/src/VOSS/chassis/AbstractChassis.cpp b/src/VOSS/chassis/AbstractChassis.cpp index 6774622a..536a7753 100644 --- a/src/VOSS/chassis/AbstractChassis.cpp +++ b/src/VOSS/chassis/AbstractChassis.cpp @@ -13,6 +13,16 @@ AbstractChassis::AbstractChassis(controller_ptr default_controller, ec_ptr ec) { this->default_ec = std::move(ec); } +void AbstractChassis::wait_until_done() const { + while (task_running) { + pros::delay(constants::MOTOR_UPDATE_DELAY); + } +} + +bool AbstractChassis::is_running() const { + return task_running; +} + void AbstractChassis::move_task(controller_ptr controller, ec_ptr ec, double max, voss::Flags flags) { @@ -154,7 +164,7 @@ 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); + pros::delay(constants::MOTOR_UPDATE_DELAY); } this->task_running = true; diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index 9b6e4024..43c696bf 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -3,6 +3,7 @@ #include "PIDController.hpp" #include "VOSS/chassis/ChassisCommand.hpp" #include "VOSS/utils/angle.hpp" +#include "VOSS/utils/debug.hpp" #include namespace voss::controller { @@ -23,18 +24,23 @@ BoomerangController::get_command(bool reverse, bool thru, Point current_pos = this->l->get_position(); int dir = reverse ? -1 : 1; - Pose trueTarget; double dx = target.x - current_pos.x; double dy = target.y - current_pos.y; double distance_error = sqrt(dx * dx + dy * dy); - double at = target.theta.value(); + double target_angle = 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}; - dx = carrotPoint.x - current_pos.x; - dy = carrotPoint.y - current_pos.y; + Point carrot_point; + if (!reverse) { + carrot_point = {target.x - distance_error * cos(target_angle) * lead_pct, + target.y - distance_error * sin(target_angle) * lead_pct}; + } else { + carrot_point = {target.x + distance_error * cos(target_angle) * lead_pct, + target.y + distance_error * sin(target_angle) * lead_pct}; + } + + dx = carrot_point.x - current_pos.x; + dy = carrot_point.y - current_pos.y; double current_angle = this->l->get_orientation_rad(); double angle_error; @@ -48,42 +54,57 @@ BoomerangController::get_command(bool reverse, bool thru, double lin_speed = linear_pid.update(distance_error); if (thru) { - lin_speed = copysign(fmax(fabs(lin_speed), this->min_vel), lin_speed); + if (thru_behavior == ThruBehavior::FULL_SPEED) { + lin_speed = 100; + } else { + lin_speed = fmax(lin_speed, min_vel); + } + } + if (cosine_scaling == CosineScaling::ALL_THE_TIME || distance_error < min_error) { + lin_speed *= cos(angle_error); } lin_speed *= dir; - double pose_error = voss::norm_delta(target.theta.value() - current_angle); - double ang_speed; if (distance_error < min_error) { this->can_reverse = true; + double pose_error = voss::norm_delta(target_angle - current_angle); - ang_speed = angular_pid.update(pose_error); - } else if (distance_error < 2 * min_error) { - double scale_factor = (distance_error - min_error) / min_error; - double scaled_angle_error = voss::norm_delta( - scale_factor * angle_error + (1 - scale_factor) * pose_error); - - ang_speed = angular_pid.update(scaled_angle_error); + if (min_err_behavior == MinErrBehavior::TARGET_HEADING) { + ang_speed = angular_pid.update(pose_error); + } else { + double min_err_2 = min_error / 2; + double scale_factor = (distance_error - min_err_2) / min_err_2; + scale_factor = fmax(0.0, scale_factor); + double scaled_angle_error = norm_delta(scale_factor * angle_error + (1 - scale_factor) * pose_error); + ang_speed = angular_pid.update(scaled_angle_error); + } } else { if (fabs(angle_error) > M_PI_2 && this->can_reverse) { angle_error = angle_error - (angle_error / fabs(angle_error)) * M_PI; - lin_speed = -lin_speed; + if (cosine_scaling != CosineScaling::ALL_THE_TIME) { + lin_speed = -lin_speed; + } } ang_speed = angular_pid.update(angle_error); } - lin_speed *= cos(angle_error); + lin_speed = std::clamp(lin_speed, -100.0, 100.0); + if (enable_overturn) { + ang_speed = std::clamp(ang_speed, -100.0, 100.0); + lin_speed = std::clamp(lin_speed, -100 + fabs(ang_speed), 100 - fabs(ang_speed)); + } - lin_speed = std::max(-100.0, std::min(100.0, lin_speed)); + if (get_debug()) { + printf("dist_err: %.2f, ang_err: %.2f, lin_speed: %.2f, ang_speed: %.2f\n", distance_error, angle_error, 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}}; + lin_speed - ang_speed, lin_speed + ang_speed}}; } else { return chassis::Stop{}; } @@ -148,4 +169,20 @@ BoomerangController::modify_lead_pct(double lead_pct) { return this->p; } +void BoomerangController::set_thru_behavior(ThruBehavior thru_behavior) { + this->thru_behavior = thru_behavior; +} + +void BoomerangController::set_cosine_scaling(CosineScaling cosine_scaling) { + this->cosine_scaling = cosine_scaling; +} + +void BoomerangController::set_min_err_behavior(MinErrBehavior min_err_behavior) { + this->min_err_behavior = min_err_behavior; +} + +void BoomerangController::set_overturn(bool overturn) { + enable_overturn = overturn; +} + } // namespace voss::controller \ No newline at end of file diff --git a/src/VOSS/controller/PIDController.cpp b/src/VOSS/controller/PIDController.cpp index 8cd953fc..08debb5b 100644 --- a/src/VOSS/controller/PIDController.cpp +++ b/src/VOSS/controller/PIDController.cpp @@ -2,6 +2,7 @@ #include "PIDControllerBuilder.hpp" #include "VOSS/chassis/ChassisCommand.hpp" #include "VOSS/utils/angle.hpp" +#include "VOSS/utils/debug.hpp" #include #include #include @@ -26,7 +27,6 @@ PIDController::get_command(bool reverse, bool thru, int dir = reverse ? -1 : 1; Point current_pos = this->l->get_position(); double current_angle = this->l->get_orientation_rad(); - bool chainedExecutable = false; bool noPose = !this->target.theta.has_value(); double dx = target.x - current_pos.x; @@ -42,16 +42,46 @@ PIDController::get_command(bool reverse, bool thru, angle_error = voss::norm_delta(angle_error); - double lin_speed = - (thru ? 100.0 : (linear_pid.update(distance_error))) * dir; + double lin_speed = linear_pid.update(distance_error); + if (thru) { + if (thru_behavior == ThruBehavior::FULL_SPEED) { + lin_speed = 100; + } else { + lin_speed = fmax(lin_speed, min_vel); + } + } + + if (cosine_scaling == CosineScaling::ALL_THE_TIME || distance_error < min_error) { + lin_speed *= cos(angle_error); + } + + lin_speed *= dir; double ang_speed; if (distance_error < min_error) { this->can_reverse = true; if (noPose) { - ang_speed = 0; // disable turning when close to the point to prevent - // spinning + switch (min_err_behavior) { + case MinErrBehavior::NO_ANG_PID: + ang_speed = 0; + break; + case MinErrBehavior::SCALE_ANG_PID: { + double min_err_2 = min_error / 2; + double scale_factor = (distance_error - min_err_2) / min_err_2; + scale_factor = fmax(0.0, scale_factor); + ang_speed = scale_factor * angular_pid.update(angle_error); + break; + } + case MinErrBehavior::MAINTAIN_HEADING: { + if (std::isnan(min_err_heading)) { + min_err_heading = current_angle; + } + double min_err_heading_error = norm_delta(min_err_heading - current_angle); + ang_speed = angular_pid.update(min_err_heading_error); + break; + } + } } else { // turn to face the finale pose angle if executing a pose movement double poseError = target.theta.value() - current_angle; @@ -61,29 +91,33 @@ PIDController::get_command(bool reverse, bool thru, ang_speed = angular_pid.update(poseError); } - // reduce the linear speed if the bot is tangent to the target - lin_speed *= cos(angle_error); - - } else if (distance_error < 2 * min_error) { - // scale angular speed down to 0 as distance_error approaches min_error - ang_speed = angular_pid.update(angle_error); - ang_speed *= (distance_error - min_error) / min_error; } else { + min_err_heading = NAN; if (fabs(angle_error) > M_PI_2 && this->can_reverse) { angle_error = angle_error - (angle_error / fabs(angle_error)) * M_PI; - lin_speed = -lin_speed; + if (cosine_scaling != CosineScaling::ALL_THE_TIME) { + lin_speed = -lin_speed; + } } ang_speed = angular_pid.update(angle_error); } - lin_speed = std::max(-100.0, std::min(100.0, lin_speed)); + lin_speed = std::clamp(lin_speed, -100.0, 100.0); + if (enable_overturn) { + ang_speed = std::clamp(ang_speed, -100.0, 100.0); + lin_speed = std::clamp(lin_speed, -100 + fabs(ang_speed), 100 - fabs(ang_speed)); + } + + if (get_debug()) { + printf("dist_err: %.2f, ang_err: %.2f, lin_speed: %.2f, ang_speed: %.2f\n", distance_error, angle_error, lin_speed, ang_speed); + } + // Runs at the end of a through movement 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}}; + lin_speed - ang_speed, lin_speed + ang_speed}}; } else { return chassis::Stop{}; } @@ -152,6 +186,7 @@ void PIDController::reset() { this->angular_pid.reset(); this->can_reverse = false; this->turn_overshoot = false; + min_err_heading = NAN; } std::shared_ptr @@ -186,4 +221,20 @@ PIDController::modify_min_error(double min_error) { return this->p; } +void PIDController::set_thru_behavior(ThruBehavior thru_behavior) { + this->thru_behavior = thru_behavior; +} + +void PIDController::set_cosine_scaling(CosineScaling cosine_scaling) { + this->cosine_scaling = cosine_scaling; +} + +void PIDController::set_min_err_behavior(MinErrBehavior min_err_behavior) { + this->min_err_behavior = min_err_behavior; +} + +void PIDController::set_overturn(bool overturn) { + enable_overturn = overturn; +} + } // 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 index 2d101026..da9e397d 100644 --- a/src/VOSS/exit_conditions/PrepLineExitCondition.cpp +++ b/src/VOSS/exit_conditions/PrepLineExitCondition.cpp @@ -1,5 +1,6 @@ #include "VOSS/exit_conditions/PrepLineExitCondition.hpp" #include "VOSS/utils/Point.hpp" +#include "VOSS/utils/debug.hpp" #include namespace voss::controller { @@ -9,39 +10,13 @@ PrepLineExitCondition::PrepLineExitCondition(double 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; + double d = voss::Point::getDistance( + {this->target_pose.x, this->target_pose.y}, {pose.x, pose.y}); + bool exit = d < this->thru_smoothness; + if (get_debug() && exit) { + printf("Thru Tolerance Condition Met\n"); } + return exit; } return false; } diff --git a/src/VOSS/exit_conditions/SettleExitCondition.cpp b/src/VOSS/exit_conditions/SettleExitCondition.cpp index fb4c1c97..2506a561 100644 --- a/src/VOSS/exit_conditions/SettleExitCondition.cpp +++ b/src/VOSS/exit_conditions/SettleExitCondition.cpp @@ -3,6 +3,7 @@ #include "VOSS/exit_conditions/AbstractExitCondition.hpp" #include "VOSS/utils/angle.hpp" #include "VOSS/utils/Pose.hpp" +#include "VOSS/utils/debug.hpp" #include #include @@ -31,7 +32,9 @@ bool SettleExitCondition::is_met(Pose current_pose, bool thru) { prev_pose = current_pose; } } else { - // printf("settle condition met\n"); + if (get_debug()) { + printf("Settle Condition Met\n"); + } return true; } return false; diff --git a/src/VOSS/exit_conditions/TimeOutExitCondition.cpp b/src/VOSS/exit_conditions/TimeOutExitCondition.cpp index d44aa615..c61cbc9e 100644 --- a/src/VOSS/exit_conditions/TimeOutExitCondition.cpp +++ b/src/VOSS/exit_conditions/TimeOutExitCondition.cpp @@ -1,5 +1,6 @@ #include "VOSS/exit_conditions/TimeOutExitCondition.hpp" #include "VOSS/constants.hpp" +#include "VOSS/utils/debug.hpp" #include namespace voss::controller { @@ -10,8 +11,10 @@ TimeOutExitCondition::TimeOutExitCondition(int timeout) : timeout(timeout) { bool TimeOutExitCondition::is_met(Pose current_pose, bool thru) { this->current_time += constants::MOTOR_UPDATE_DELAY; - // if (this->current_time >= this->timeout) - // printf("Time out cond met\n"); + bool exit = current_time >= timeout; + if (get_debug() && exit) { + printf("Timeout Condition Met\n"); + } return this->current_time >= this->timeout; } diff --git a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp index 3614cca6..34a43152 100644 --- a/src/VOSS/exit_conditions/ToleranceExitCondition.cpp +++ b/src/VOSS/exit_conditions/ToleranceExitCondition.cpp @@ -1,17 +1,22 @@ #include "VOSS/exit_conditions/ToleranceExitCondition.hpp" +#include "VOSS/utils/debug.hpp" namespace voss::controller { bool ToleranceExitCondition::is_met(Pose pose, bool thru) { + bool exit = false; if (this->target_has_coordinate() && this->target_has_heading()) { - return ang_exit->is_met(pose, thru) && lin_exit->is_met(pose, thru); + exit = ang_exit->is_met(pose, thru) && lin_exit->is_met(pose, thru); } else if (this->target_has_coordinate()) { - return lin_exit->is_met(pose, thru); + exit = lin_exit->is_met(pose, thru); } else if (this->target_has_heading()) { - return ang_exit->is_met(pose, thru); + exit = ang_exit->is_met(pose, thru); } - return false; + if (get_debug() && exit) { + printf("Tolerance Condition Met\n"); + } + return exit; } void ToleranceExitCondition::set_target(Pose target) { diff --git a/src/VOSS/localizer/AbstractLocalizer.cpp b/src/VOSS/localizer/AbstractLocalizer.cpp index a43ac608..c0dcf74b 100644 --- a/src/VOSS/localizer/AbstractLocalizer.cpp +++ b/src/VOSS/localizer/AbstractLocalizer.cpp @@ -1,6 +1,7 @@ #include "VOSS/localizer/AbstractLocalizer.hpp" #include "VOSS/constants.hpp" #include "VOSS/utils/angle.hpp" +#include "VOSS/utils/debug.hpp" #include namespace voss::localizer { @@ -15,11 +16,16 @@ AbstractLocalizer::AbstractLocalizer() { // Once started it don't stop until program is stopped or data abort // Uses mutex to keep values protected void AbstractLocalizer::begin_localization() { + this->calibrate(); pros::Task localization_task([this]() { - this->calibrate(); + int print_counter = 3; while (true) { std::unique_lock lock(this->mtx); this->update(); + if (get_debug() && (--print_counter) <= 0) { + print_counter = 3; + printf("X: %.2f, Y: %.2f, H: %.2f\n", pose.x.load(), pose.y.load(), to_degrees(pose.theta.load())); + } lock.unlock(); pros::delay(constants::SENSOR_UPDATE_DELAY); @@ -80,4 +86,21 @@ Point AbstractLocalizer::get_position() { return ret; } +void AbstractLocalizer::wait_until_near(Point target, double tolerance) { + while (Point::getDistance(target, get_position()) > tolerance) { + pros::delay(constants::SENSOR_UPDATE_DELAY); + } +} + +void AbstractLocalizer::wait_until_distance(double distance) { + double dist_travelled = 0; + Point prev_point = get_position(); + while (dist_travelled < distance) { + pros::delay(constants::SENSOR_UPDATE_DELAY); + Point curr_point = get_position(); + dist_travelled += Point::getDistance(curr_point, prev_point); + prev_point = curr_point; + } +} + } // namespace voss::localizer \ No newline at end of file diff --git a/src/VOSS/localizer/TrackingWheelLocalizer.cpp b/src/VOSS/localizer/TrackingWheelLocalizer.cpp index 1393a8f0..964e5518 100644 --- a/src/VOSS/localizer/TrackingWheelLocalizer.cpp +++ b/src/VOSS/localizer/TrackingWheelLocalizer.cpp @@ -34,17 +34,17 @@ void TrackingWheelLocalizer::update() { middle_tracking_wheel->get_dist_travelled() - prev_middle_pos; } if (imu) { - pose.theta = -to_radians(imu->get_rotation()); - delta_angle = pose.theta - prev_pose.theta; + real_pose.theta = -to_radians(imu->get_rotation()); + delta_angle = real_pose.theta - prev_pose.theta; } else { delta_angle = (delta_right - delta_left) / (2 * left_right_dist); - pose.theta += delta_angle; + real_pose.theta += delta_angle; } prev_left_pos += delta_left; prev_right_pos += delta_right; prev_middle_pos += delta_middle; - prev_pose = pose; + prev_pose = real_pose; double local_x; double local_y; @@ -64,11 +64,15 @@ void TrackingWheelLocalizer::update() { local_y = delta_middle; } - double p = this->pose.theta - delta_angle / 2.0; // global angle + double p = this->real_pose.theta - delta_angle / 2.0; // global angle // convert to absolute displacement - this->pose.x += cos(p) * local_x - sin(p) * local_y; - this->pose.y += sin(p) * local_x + cos(p) * local_y; + this->real_pose.x += cos(p) * local_x - sin(p) * local_y; + this->real_pose.y += sin(p) * local_x + cos(p) * local_y; + + this->pose.theta = this->real_pose.theta - local_offset.theta.value(); + this->pose.x = this->real_pose.x - (local_offset.x * cos(pose.theta) - local_offset.y * sin(pose.theta)); + this->pose.y = this->real_pose.y - (local_offset.x * sin(pose.theta) + local_offset.y * cos(pose.theta)); } void TrackingWheelLocalizer::calibrate() { @@ -92,9 +96,12 @@ void TrackingWheelLocalizer::calibrate() { void TrackingWheelLocalizer::set_pose(Pose pose) { this->AbstractLocalizer::set_pose(pose); - this->prev_pose = this->pose; - if (this->imu && pose.theta.has_value()) { - this->imu->set_rotation(-pose.theta.value()); + this->real_pose.theta = this->pose.theta + local_offset.theta.value(); + this->real_pose.x = pose.x + (local_offset.x * cos(this->pose.theta) - local_offset.y * sin(this->pose.theta)); + this->real_pose.y = pose.y + (local_offset.x * sin(this->pose.theta) + local_offset.y * cos(this->pose.theta)); + this->prev_pose = this->real_pose; + if (this->imu) { + this->imu->set_rotation(-to_degrees(real_pose.theta)); } } @@ -102,4 +109,9 @@ void TrackingWheelLocalizer::set_pose(double x, double y, double theta) { this->set_pose({x, y, theta}); } +void TrackingWheelLocalizer::set_local_offset(Pose local_offset) { + this->local_offset = {local_offset.x, local_offset.y, to_radians(local_offset.theta.value())}; + set_pose(get_pose()); +} + } // namespace voss::localizer \ No newline at end of file diff --git a/src/VOSS/utils/debug.cpp b/src/VOSS/utils/debug.cpp new file mode 100644 index 00000000..28889e6b --- /dev/null +++ b/src/VOSS/utils/debug.cpp @@ -0,0 +1,18 @@ +#include "VOSS/utils/debug.hpp" + +namespace voss { + +bool debug; + +void enable_debug() { + debug = true; +} + +void disable_debug() { + debug = false; +} + +bool get_debug() { + return debug; +} +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 32bb1917..fcc86a4d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -7,31 +7,33 @@ #include "VOSS/utils/flags.hpp" #define LEFT_MOTORS \ - { -4, -1, -21, 8, 13 } + { -10, -12, 14, -15, 20 } #define RIGHT_MOTORS \ - { 10, 3, 9, -7, -15 } + { 1, 2, -3, -4, 5 } auto odom = voss::localizer::TrackingWheelLocalizerBuilder::new_builder() - .with_right_motor(10) - .with_left_motor(-4) - .with_track_width(11) - .with_left_right_tpi(18.43) - .with_imu(16) + .with_left_encoder(3) + .with_middle_encoder(1) + .with_imu(19) + .with_left_right_tpi(522) + .with_middle_tpi(522) + .with_track_width(2) + .with_middle_dist(1.5) .build(); auto pid = voss::controller::PIDControllerBuilder::new_builder(odom) - .with_linear_constants(20, 0.02, 169) - .with_angular_constants(250, 0.05, 2435) + .with_linear_constants(8, 0, 70) + .with_angular_constants(250, 0.001, 2500) .with_min_error(5) - .with_min_vel_for_thru(100) + .with_min_vel_for_thru(40) .build(); auto boomerang = voss::controller::BoomerangControllerBuilder::new_builder(odom) - .with_linear_constants(20, 0.02, 169) - .with_angular_constants(250, 0.05, 2435) - .with_lead_pct(0.5) + .with_linear_constants(8, 0, 70) + .with_angular_constants(250, 0.001, 2500) + .with_lead_pct(0.6) .with_min_vel_for_thru(70) - .with_min_error(5) + .with_min_error(10) .build(); auto swing = voss::controller::SwingControllerBuilder::new_builder(odom) @@ -59,8 +61,6 @@ auto ec = voss::controller::ExitConditions::new_conditions() auto chassis = voss::chassis::DiffChassis(LEFT_MOTORS, RIGHT_MOTORS, pid, ec, 8, pros::E_MOTOR_BRAKE_COAST); -pros::IMU imu(16); - /** * Runs initialization code. This occurs as soon as the program is started. * @@ -140,8 +140,8 @@ 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({-24, 24}, arc); + odom->set_pose({0.0, 0.0, 0}); + chassis.move({36, 36, 90}, boomerang, 70); } pros::lcd::clear_line(1); From 5c4d3c48211c798bcbe8ffc0891bc990b0463a2e Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Mon, 31 Mar 2025 15:50:56 -0400 Subject: [PATCH 2/4] print carrot point in boomerang debug --- src/VOSS/controller/BoomerangController.cpp | 2 +- src/main.cpp | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/VOSS/controller/BoomerangController.cpp b/src/VOSS/controller/BoomerangController.cpp index 43c696bf..a6060c48 100644 --- a/src/VOSS/controller/BoomerangController.cpp +++ b/src/VOSS/controller/BoomerangController.cpp @@ -98,7 +98,7 @@ BoomerangController::get_command(bool reverse, bool thru, } if (get_debug()) { - printf("dist_err: %.2f, ang_err: %.2f, lin_speed: %.2f, ang_speed: %.2f\n", distance_error, angle_error, lin_speed, ang_speed); + printf("carrot point: %.2f, %.2f, dist_err: %.2f, ang_err: %.2f, lin_speed: %.2f, ang_speed: %.2f\n", carrot_point.x, carrot_point.y, angle_error, lin_speed, ang_speed); } if (ec->is_met(this->l->get_pose(), thru)) { diff --git a/src/main.cpp b/src/main.cpp index fcc86a4d..258be7df 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -141,7 +141,9 @@ void opcontrol() { if (master.get_digital_new_press(DIGITAL_Y)) { odom->set_pose({0.0, 0.0, 0}); - chassis.move({36, 36, 90}, boomerang, 70); + voss::enable_debug(); + chassis.move({-36, -36, 90}, boomerang, 70, voss::Flags::REVERSE); + voss::disable_debug(); } pros::lcd::clear_line(1); From 43ea781dc7eb4ada25da31f0cd9f5fe413396490 Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Mon, 31 Mar 2025 17:15:17 -0400 Subject: [PATCH 3/4] update clang-tidy action --- .github/workflows/clang-tidy.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml index 62aaf8cd..36dc1d88 100644 --- a/.github/workflows/clang-tidy.yml +++ b/.github/workflows/clang-tidy.yml @@ -25,4 +25,4 @@ jobs: - name: Build Project run: pros build-compile-commands - name: Run clang-tidy - uses: ZedThree/clang-tidy-review@v0.14.0 \ No newline at end of file + uses: ZedThree/clang-tidy-review@v0.20.1 \ No newline at end of file From 1c6face2ec6ad9c2963e6d654365cdecb4853f38 Mon Sep 17 00:00:00 2001 From: Andrew Lu Date: Tue, 29 Apr 2025 20:19:46 -0400 Subject: [PATCH 4/4] bump version number --- Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile b/Makefile index cf920d53..80ee609c 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:= IS_LIBRARY:=1 # TODO: CHANGE THIS! LIBNAME:=VOSS -VERSION:=0.1.4 +VERSION:=0.1.5 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c # this line excludes opcontrol.c and similar files EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext)))