From a7847164cdd4f22cbb9bffa35a65ace06956e5aa Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Tue, 26 Nov 2024 15:25:55 -0800 Subject: [PATCH] Cleaning everything up --- src/EZ-Template/drive/drive.cpp | 6 ++--- src/EZ-Template/drive/pid_tasks.cpp | 36 ----------------------------- src/autons.cpp | 8 +++---- src/main.cpp | 31 +++++++++++++++---------- 4 files changed, 26 insertions(+), 55 deletions(-) diff --git a/src/EZ-Template/drive/drive.cpp b/src/EZ-Template/drive/drive.cpp index 9ee11ce9..ed750760 100644 --- a/src/EZ-Template/drive/drive.cpp +++ b/src/EZ-Template/drive/drive.cpp @@ -199,7 +199,7 @@ void Drive::drive_defaults_set() { pid_heading_constants_set(11.0, 0.0, 20.0); pid_turn_constants_set(3.0, 0.05, 20.0, 15.0); pid_swing_constants_set(6.0, 0.0, 65.0); - pid_odom_angular_constants_set(5.5, 0.0, 62.5); + pid_odom_angular_constants_set(5.0, 0.0, 60.0); pid_odom_boomerang_constants_set(3.5, 0.0, 35.0); pid_turn_min_set(30); pid_swing_min_set(30); @@ -213,8 +213,8 @@ void Drive::drive_defaults_set() { pid_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); pid_swing_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 500_ms); pid_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 500_ms); - pid_odom_turn_exit_condition_set(80_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); - pid_odom_drive_exit_condition_set(80_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms); + pid_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); + pid_odom_drive_exit_condition_set(90_ms, 1_in, 250_ms, 3_in, 500_ms, 750_ms); pid_turn_chain_constant_set(3_deg); pid_swing_chain_constant_set(5_deg); diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index f95bf702..c520454d 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -157,13 +157,10 @@ void Drive::swing_pid_task() { // Odom To Point Task void Drive::ptp_task() { - bool print_debug = false; - // Compute slew slew_left.iterate(drive_sensor_left()); slew_right.iterate(drive_sensor_right()); double max_slew_out = fmax(slew_left.output(), slew_right.output()); - // double new_max_slew_out = max_slew_out * 2.0; // Decide if we've past the target or not int dir = (current_drive_direction == REV ? -1 : 1); // If we're going backwards, add a -1 @@ -181,23 +178,14 @@ void Drive::ptp_task() { current_a_odomPID.compute_error(wrapped_a_target, odom_theta_get()); // printf("shortest_a_target: %.2f error: %.2f\n", a_target, wrapped_a_target); - // if (!print_debug) printf("kp: %.2f ki: %.5f kd: %.2f start_i: %.2f thratio: %.2f hratio: %.2f tratio: %.2f angle err: %.2f\n", kp, ki, kd, kstart_i, turn_heading_ratio, heading_ratio, turning_ratio, current_a_odomPID.error); - - double rawxyout = 0.0; - double rawaout = 0.0; - // Prioritize turning by scaling xy_out down double xy_out = xyPID.output; xy_out = util::clamp(xy_out, max_slew_out); - if (print_debug) printf("xyt(%.2f, %.2f, %.2f)", odom_x_get(), odom_y_get(), odom_theta_get()); - rawxyout = xy_out; // double scale = cos(util::to_rad(current_a_odomPID.error)) / odom_turn_bias_amount; double scale = 1.0 - ((1.0 - cos(util::to_rad(current_a_odomPID.error))) / odom_turn_bias_amount); // 1 - ((1-0.7)/0.75) if (is_odom_turn_bias_enabled) xy_out *= scale; - if (print_debug) printf(" scaled xy out by %.2f: %.2f", scale, xy_out); double a_out = current_a_odomPID.output; - rawaout = a_out; // a_out = util::clamp(a_out, max_slew_out); // Scale xy_out and a_out to max speed @@ -208,15 +196,10 @@ void Drive::ptp_task() { a_out *= (max_slew_out / faster_side); } - if (print_debug) printf(" raw xy/a(%.2f, %.2f)", rawxyout, rawaout); - if (print_debug) printf(" final xy/a(%.2f, %.2f)", xy_out, a_out); - // Combine heading and drive double l_out = xy_out + a_out; double r_out = xy_out - a_out; - if (print_debug) printf(" raw lr(%.2f, %.2f)", l_out, r_out); - // Vector scaling when combining drive and imu // this ensures no data is lost that would otherwise by lost in clamping faster_side = fmax(fabs(l_out), fabs(r_out)); @@ -225,25 +208,6 @@ void Drive::ptp_task() { r_out *= (max_slew_out / faster_side); } - /* - double forward = xy_out / max_slew_out; - double curve = a_out / max_slew_out; - double left_speed = forward + fabs(forward) * curve; - double right_speed = forward - fabs(forward) * curve; - // normalizes output - double fasterr_side = fmax(fabs(left_speed), fabs(right_speed)); - if (fasterr_side > 1.0) { - left_speed /= fasterr_side; - right_speed /= fasterr_side; - } - l_out = left_speed * max_slew_out; - r_out = right_speed * max_slew_out; - */ - - if (print_debug) printf(" final lr(%.2f, %.2f)", l_out, r_out); - - if (print_debug) printf(" a err: %.2f\n", current_a_odomPID.error); - // printf("lr out (%.2f, %.2f) fwd curveZ(%.2f, %.2f) lr slew (%.2f, %.2f)\n", l_out, r_out, xy_out, a_out, slew_left.output(), slew_right.output()); // printf("max_slew_out %.2f headingerr: %.2f\n", max_slew_out, aPID.error); // printf("lr(%.2f, %.2f) xy_raw: %.2f xy_out: %.2f heading_out: %.2f max_slew_out: %.2f\n", l_out, r_out, xyPID.output, xy_out, current_a_odomPID.output, max_slew_out); diff --git a/src/autons.cpp b/src/autons.cpp index adfe1b2e..b3f31701 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -20,16 +20,16 @@ void default_constants() { chassis.pid_heading_constants_set(11.0, 0.0, 20.0); // Holds the robot straight while going forward without odom chassis.pid_turn_constants_set(3.0, 0.05, 20.0, 15.0); // Turn in place constants chassis.pid_swing_constants_set(6.0, 0.0, 65.0); // Swing constants - chassis.pid_odom_angular_constants_set(5.5, 0.0, 62.5); // Angular control for odom motions // 5.5 62.5 // 6 72.5 - chassis.pid_odom_boomerang_constants_set(3.5, 0.0, 35.0); // Angular control for boomerang motions // 5.7 65.0 // 5.5 62.5 // 6 72.5 + chassis.pid_odom_angular_constants_set(5.0, 0.0, 60.0); // Angular control for odom motions + chassis.pid_odom_boomerang_constants_set(3.5, 0.0, 35.0); // Angular control for boomerang motions // 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_odom_turn_exit_condition_set(90_ms, 3_deg, 250_ms, 7_deg, 500_ms, 750_ms); + chassis.pid_odom_drive_exit_condition_set(90_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); diff --git a/src/main.cpp b/src/main.cpp index 1a2543c2..aab6c831 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,7 +22,7 @@ ez::Drive chassis( {-5, -6, -7, -8}, // Left Chassis Ports (negative port will reverse it!) {11, 15, 16, 17}, // Right Chassis Ports (negative port will reverse it!) - 21, // IMU port + 21, // IMU Port 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) 420.0); // Wheel RPM = cartridge * (motor gear / wheel gear) @@ -117,25 +117,32 @@ void autonomous() { chassis.pid_targets_reset(); // Resets PID targets to 0 chassis.drive_imu_reset(); // Reset gyro position to 0 chassis.drive_sensor_reset(); // Reset drive sensors to 0 - chassis.odom_pose_set({0_in, 0_in, 0_deg}); // Reset the current position to 0 + chassis.odom_pose_set({0_in, 0_in, 0_deg}); // Set the current position, you can start at a specific position with this chassis.drive_brake_set(MOTOR_BRAKE_HOLD); // Set motors to hold. This helps autonomous consistency - chassis.drive_width_set(12.0_in); + chassis.drive_width_set(12.0_in); // You can measure this with a tape measure - int speed = 110; + // Drive to 0, 24 + chassis.pid_odom_set({{0_in, 24_in}, fwd, 110}, + true); + chassis.pid_wait(); - // chassis.pid_odom_set({{0_in, 24_in, 90_deg}, fwd, speed}); + // Turn to face 24, 24 + chassis.pid_turn_set({24_in, 24_in}, fwd, 110, + true); + chassis.pid_wait(); - chassis.pid_odom_set({{{0_in, 24_in}, fwd, speed}, - {{24_in, 24_in}, fwd, speed}}, + // Drive to 24, 24 + chassis.pid_odom_set({{24_in, 24_in}, fwd, 110}, true); - // chassis.pid_odom_set({{{16_in, 16_in}, fwd, speed}}, true); chassis.pid_wait(); - pros::delay(250); - // chassis.pid_odom_set({{24_in, 24_in, 90_deg}, fwd, speed}, true); - // chassis.pid_wait(); - // pros::delay(250); + // Drive backwards through 0, 24 and stop at 0, 0 + chassis.pid_odom_set({{{0_in, 24_in}, rev, 110}, + {{0_in, 0_in}, rev, 110}}, + true); + chassis.pid_wait(); + // Uncomment this to use the auton selector // ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector }