From d14f05c5ac785891a9900c019327debe8bb23509 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 3 Jul 2024 23:06:30 -0700 Subject: [PATCH] Boomerang anchoring added to normal pure pursuit, fixed wait until point --- src/EZ-Template/drive/exit_conditions.cpp | 17 +++++++-- src/EZ-Template/drive/purepursuit_math.cpp | 11 +++++- src/EZ-Template/drive/set_pid.cpp | 42 +++++++++++++--------- 3 files changed, 51 insertions(+), 19 deletions(-) diff --git a/src/EZ-Template/drive/exit_conditions.cpp b/src/EZ-Template/drive/exit_conditions.cpp index a0307c15..30e1dd4e 100644 --- a/src/EZ-Template/drive/exit_conditions.cpp +++ b/src/EZ-Template/drive/exit_conditions.cpp @@ -353,10 +353,23 @@ void Drive::pid_wait_until_pp(int index) { if (index > injected_pp_index.size() || index < 0) printf(" Wait Until PP Error! Index %i is not within range! %i is max!\n", index, injected_pp_index.size()); - while (pp_index < injected_pp_index[index]) { + exit_output xy_exit = RUNNING; + exit_output a_exit = RUNNING; + while (pp_index < injected_pp_index[index + 1]) { + xyPID.velocity_sensor_secondary_set(drive_imu_accel_get()); + aPID.velocity_sensor_secondary_set(drive_imu_accel_get()); + xy_exit = xy_exit != RUNNING ? xy_exit : xyPID.exit_condition({left_motors[0], right_motors[0]}); + a_exit = a_exit != RUNNING ? a_exit : aPID.exit_condition({left_motors[0], right_motors[0]}); + + if (xy_exit == mA_EXIT || xy_exit == VELOCITY_EXIT || a_exit == mA_EXIT || a_exit == VELOCITY_EXIT) + break; + + printf("%i < %i\n", pp_index, injected_pp_index[index]); + pros::delay(util::DELAY_TIME); } - if (print_toggle) printf(" Wait Until PP at (%f, %f)\n", pp_movements[pp_index].target.x, pp_movements[pp_index].target.y); + + if (print_toggle) printf(" Wait Until PP at (%.2f, %.2f)\n", pp_movements[pp_index].target.x, pp_movements[pp_index].target.y); } // Pid wait, but quickly :) diff --git a/src/EZ-Template/drive/purepursuit_math.cpp b/src/EZ-Template/drive/purepursuit_math.cpp index 838a25ac..07461973 100644 --- a/src/EZ-Template/drive/purepursuit_math.cpp +++ b/src/EZ-Template/drive/purepursuit_math.cpp @@ -83,6 +83,7 @@ std::vector Drive::inject_points(std::vector imovements) { std::vector output; // Output vector int output_index = -1; // Keeps track of current index + injected_pp_index.push_back(0); // This for loop runs for how many points there are minus one because there is one less vector then points for (int i = 0; i < input.size() - 1; i++) { @@ -95,7 +96,11 @@ std::vector Drive::inject_points(std::vector imovements) { input[i].turn_type, input[i].max_xy_speed}); output_index++; - injected_pp_index.push_back(output_index); + if (i != 0 && input[i - 1].target.theta == ANGLE_NOT_SET) { + if (input[i].target.theta == ANGLE_NOT_SET) { + injected_pp_index.push_back(output_index); + } + } // Add the injected points for (int j = 0; j < num_of_points_that_fit; j++) { @@ -111,6 +116,10 @@ std::vector Drive::inject_points(std::vector imovements) { input[i + 1].turn_type, input[i + 1].max_xy_speed}); output_index++; + + if (j == 0 && input[i].target.theta != ANGLE_NOT_SET) { + injected_pp_index.push_back(output_index); + } } } else { j = num_of_points_that_fit; diff --git a/src/EZ-Template/drive/set_pid.cpp b/src/EZ-Template/drive/set_pid.cpp index 1add9b73..d8c85594 100644 --- a/src/EZ-Template/drive/set_pid.cpp +++ b/src/EZ-Template/drive/set_pid.cpp @@ -324,7 +324,7 @@ void Drive::pid_turn_set(pose itarget, turn_types dir, int speed, bool slew_on) // Calculate the point to look at point_to_face = find_point_to_face(odom_current, {itarget.x, itarget.y}, true); - int add = current_drive_direction == REV ? 180 : 0; // Decide if going fwd or rev + int add = current_drive_direction == REV ? 180 : 0; // Decide if going fwd or rev double target = util::absolute_angle_to_point(point_to_face[!ptf1_running], odom_current) + add; // Calculate the point for angle to face turnPID.target_set(util::wrap_angle(target - drive_imu_get())); // Constrain error to -180 to 180 @@ -426,24 +426,40 @@ void Drive::raw_pid_odom_pp_set(std::vector imovements, bool slew_on) { // Pure pursuit void Drive::pid_odom_pp_set(std::vector imovements, bool slew_on) { + std::vector input = imovements; + input.insert(input.begin(), {{{odom_current.x, odom_current.y, ANGLE_NOT_SET}, imovements[0].turn_type, imovements[0].max_xy_speed}}); + + int t = 0; + for (int i = 0; i < input.size() - 1; i++) { + int j = i + t; + j = i; + if (input[j].target.theta != ANGLE_NOT_SET) { + // Calculate the new point with known information: hypot and angle + double angle_to_point = input[j].target.theta; + int dir = input[j].turn_type == REV ? -1 : 1; + pose new_point = util::vector_off_point(LOOK_AHEAD * dir, {input[j].target.x, input[j].target.y, angle_to_point}); + new_point.theta = ANGLE_NOT_SET; + + input.insert(input.cbegin() + j + 1, {new_point, input[j].turn_type, input[j].max_xy_speed}); + + t++; + } + } + // This is used for pid_wait_until_pp() injected_pp_index.clear(); - for (int i = 0; i < imovements.size(); i++) { - injected_pp_index.push_back(i); + injected_pp_index.push_back(0); + for (int i = 0; i < input.size(); i++) { + if (input[i].target.theta == ANGLE_NOT_SET && i != 0) + injected_pp_index.push_back(i); } if (print_toggle) printf("Pure Pursuit "); - raw_pid_odom_pp_set(imovements, slew_on); + raw_pid_odom_pp_set(input, slew_on); } // Smooth injected pure pursuit void Drive::pid_odom_injected_pp_set(std::vector imovements, bool slew_on) { - // This is used for pid_wait_until_pp() - injected_pp_index.clear(); - for (int i = 0; i < imovements.size(); i++) { - injected_pp_index.push_back(i); - } - if (print_toggle) printf("Injected "); std::vector input_path = inject_points(imovements); raw_pid_odom_pp_set(input_path, slew_on); @@ -451,12 +467,6 @@ void Drive::pid_odom_injected_pp_set(std::vector imovements, bool slew // Smooth injected pure pursuit void Drive::pid_odom_smooth_pp_set(std::vector imovements, bool slew_on) { - // This is used for pid_wait_until_pp() - injected_pp_index.clear(); - for (int i = 0; i < imovements.size(); i++) { - injected_pp_index.push_back(i); - } - if (print_toggle) printf("Smooth Injected "); std::vector input_path = smooth_path(inject_points(imovements), 0.75, 0.03, 0.0001); raw_pid_odom_pp_set(input_path, slew_on);