From 1e94aa30dcd7fa1aa21f6056ba5790f37c65d4ea Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 4 Sep 2024 17:08:11 -0700 Subject: [PATCH] Added missing functions and some simple refactoring --- include/EZ-Template/drive/drive.hpp | 10 ++++++- src/EZ-Template/drive/pid_tasks.cpp | 4 +-- .../drive/set_pid/set_odom_pid.cpp | 6 +++++ src/EZ-Template/drive/tracking.cpp | 26 +++++-------------- src/autons.cpp | 7 ++++- src/main.cpp | 2 -- 6 files changed, 30 insertions(+), 25 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index a60332f..c5b794f 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -426,7 +426,15 @@ class Drive { void pid_wait_until_index(int index); void pid_wait_until_point(pose target); void pid_wait_until(pose target); - double dlead = 0.375; + double dlead = 0.5; + void odom_boomerang_dlead_set(double input); + double odom_boomerang_dlead_get(); + double max_boomerang_distance = 12.0; + void odom_boomerang_distance_set(double distance); + double odom_boomerang_distance_get(); + double odom_turn_bias_amount = 1.375; + void odom_turn_bias_set(double bias); + double odom_turn_bias_get(); // Odometry bool odometry_enabled = true; float track_width = 0.0; diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index 65df464..c2ac1b0 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -184,7 +184,7 @@ void Drive::ptp_task() { // Prioritize turning by scaling xy_out down double xy_out = xyPID.output; // xy_out = util::clamp(xy_out, max_slew_out, -max_slew_out); - xy_out *= cos(util::to_rad(aPID.error)) / 1.375; + xy_out *= cos(util::to_rad(aPID.error)) / odom_turn_bias_amount; double a_out = aPID.output; // a_out = util::clamp(a_out, max_slew_out, -max_slew_out); @@ -231,7 +231,7 @@ void Drive::boomerang_task() { int dir = current_drive_direction == REV ? -1 : 1; double h = util::distance_to_point(target, odom_current) * dlead; - double max = 12.0; + double max = max_boomerang_distance; h = h > max ? max : h; h *= dir; diff --git a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp index 6ea1f8b..26fcab6 100644 --- a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp @@ -44,6 +44,12 @@ void Drive::odom_x_direction_flip(bool flip) { x_flipped = flip; } bool Drive::odom_x_direction_get() { return x_flipped; } void Drive::odom_y_direction_flip(bool flip) { y_flipped = flip; } bool Drive::odom_y_direction_get() { return y_flipped; } +void Drive::odom_boomerang_dlead_set(double input) { dlead = input; } +double Drive::odom_boomerang_dlead_get() { return dlead; } +void Drive::odom_boomerang_distance_set(double distance) { max_boomerang_distance = distance; } +double Drive::odom_boomerang_distance_get() { return max_boomerang_distance; } +void Drive::odom_turn_bias_set(double bias) { odom_turn_bias_amount = 1.375; } +double Drive::odom_turn_bias_get() { return odom_turn_bias_amount; } ///// // pid_odom_set diff --git a/src/EZ-Template/drive/tracking.cpp b/src/EZ-Template/drive/tracking.cpp index 955a915..de9d9e6 100644 --- a/src/EZ-Template/drive/tracking.cpp +++ b/src/EZ-Template/drive/tracking.cpp @@ -8,31 +8,19 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. using namespace ez; -void Drive::odom_pose_x_set(double x) { - // odom_target.x = x; - odom_current.x = x; -} -void Drive::odom_pose_y_set(double y) { - // odom_target.y = y; - odom_current.y = y; -} - +void Drive::odom_pose_x_set(double x) { odom_current.x = x; } +void Drive::odom_pose_y_set(double y) { odom_current.y = y; } +void Drive::odom_pose_theta_set(double a) { drive_angle_set(a); } +void Drive::odom_reset() { odom_pose_set({0, 0, 0}); } +void Drive::drive_width_set(double input) { track_width = input; } +double Drive::drive_width_get() { return track_width; } +void Drive::drive_odom_enable(bool input) { odometry_enabled = input; } void Drive::odom_pose_set(pose itarget) { odom_pose_theta_set(itarget.theta); odom_pose_x_set(itarget.x); odom_pose_y_set(itarget.y); } -void Drive::odom_pose_theta_set(double a) { drive_angle_set(a); } -void Drive::odom_reset() { odom_pose_set({0, 0, 0}); } - -void Drive::drive_width_set(double input) { - track_width = input; -} -double Drive::drive_width_get() { return track_width; } - -void Drive::drive_odom_enable(bool input) { odometry_enabled = input; } - // Tracking based on https://wiki.purduesigbots.com/software/odometry void Drive::ez_tracking_task() { if (!imu_calibration_complete || !odometry_enabled) { diff --git a/src/autons.cpp b/src/autons.cpp index 46af328..2530eb2 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -14,21 +14,26 @@ const int SWING_SPEED = 110; // Constants /// void default_constants() { + // P, I, D, and Start I + // https://ez-robotics.github.io/EZ-Template/tutorials/tuning_constants chassis.pid_heading_constants_set(7, 0, 45); chassis.pid_drive_constants_set(20, 0, 100); chassis.pid_turn_constants_set(3, 0.05, 20, 15); chassis.pid_swing_constants_set(6, 0, 65); + // Exit conditions + // https://ez-robotics.github.io/EZ-Template/tutorials/tuning_exit_conditions chassis.pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); chassis.pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); chassis.pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); chassis.pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); chassis.pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms); - chassis.pid_turn_chain_constant_set(3_deg); chassis.pid_swing_chain_constant_set(5_deg); chassis.pid_drive_chain_constant_set(3_in); + // Slew constants + // https://ez-robotics.github.io/EZ-Template/tutorials/slew_constants chassis.slew_turn_constants_set(3_deg, 70); chassis.slew_drive_constants_set(3_in, 70); chassis.slew_swing_constants_set(3_in, 80); diff --git a/src/main.cpp b/src/main.cpp index 4978813..350ac0e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -97,8 +97,6 @@ void autonomous() { chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This helps autonomous consistency chassis.odom_pose_set({0, 0, 0}); chassis.drive_width_set(11); // just use a tape measure - chassis.dlead = 0.5; - chassis.odometry_enabled = true; chassis.pid_odom_set({{{0_in, 16_in}, fwd, 110}, {{16_in, 16_in}, fwd, 110}},