diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 8397491..ec77384 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -358,28 +358,30 @@ class Drive { void pid_odom_turn_exit_condition_set(int p_small_exit_time, double p_small_error, int p_big_exit_time, double p_big_error, int p_velocity_exit_time, int p_mA_timeout, bool use_imu = true); void pid_odom_turn_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QAngle p_small_error, okapi::QTime p_big_exit_time, okapi::QAngle p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu = true); void pid_odom_drive_exit_condition_set(okapi::QTime p_small_exit_time, okapi::QLength p_small_error, okapi::QTime p_big_exit_time, okapi::QLength p_big_error, okapi::QTime p_velocity_exit_time, okapi::QTime p_mA_timeout, bool use_imu = true); + void drive_width_set(okapi::QLength p_input); void drive_width_set(double input); double drive_width_get(); + void drive_odom_enable(bool input); - pose odom_target = {0, 0, 0}; - pose odom_current = {0, 0, 0}; - pose odom_second_to_last = {0, 0, 0}; - pose odom_start = {0, 0, 0}; - pose odom_target_start = {0, 0, 0}; + bool drive_odom_enabled(); + std::vector pp_movements; std::vector injected_pp_index; int pp_index = 0; void odom_x_set(double x); void odom_y_set(double y); - void odom_pose_set(pose itarget); void odom_theta_set(double a); + void odom_x_set(okapi::QLength p_x); + void odom_y_set(okapi::QLength p_y); + void odom_theta_set(okapi::QAngle p_a); + void odom_pose_set(pose itarget); + void odom_pose_set(united_pose itarget); void odom_reset(); double odom_x_get(); double odom_y_get(); double odom_theta_get(); pose odom_pose_get(); bool imu_calibration_complete = false; - double angle_rad = 0.0; void pid_turn_set(pose itarget, drive_directions dir, int speed); void pid_turn_set(pose itarget, drive_directions dir, int speed, bool slew_on); @@ -390,13 +392,16 @@ class Drive { void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior); void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); - pose turn_to_point_target = {0, 0, 0}; - void pid_odom_ptp_set(odom imovement); void pid_odom_ptp_set(odom imovement, bool slew_on); void pid_odom_ptp_set(united_odom p_imovement); void pid_odom_ptp_set(united_odom p_imovement, bool slew_on); + void pid_odom_set(double target, int speed); + void pid_odom_set(okapi::QLength p_target, int speed, bool slew_on); + void pid_odom_set(okapi::QLength p_target, int speed); + void pid_odom_set(double target, int speed, bool slew_on); + void pid_odom_set(odom imovement); void pid_odom_set(odom imovement, bool slew_on); void pid_odom_set(std::vector imovements); @@ -426,47 +431,44 @@ class Drive { void pid_odom_boomerang_set(united_odom p_imovement); void pid_odom_boomerang_set(united_odom p_imovement, bool slew_on); - std::vector set_odoms_direction(std::vector inputs); - odom set_odom_direction(odom input); - pose flip_pose(pose input); // true means left is positive x, false means right is positive x void odom_x_direction_flip(bool flip = true); bool odom_x_direction_get(); // true means down is positive y, false means up is positive y void odom_y_direction_flip(bool flip = true); bool odom_y_direction_get(); - bool y_flipped = false; - bool x_flipped = false; - double odom_imu_start = 0.0; std::vector smooth_path(std::vector ipath, double weight_smooth = 0.75, double weight_data = 0.03, double tolerance = 0.0001); double is_past_target(pose target, pose current); void raw_pid_odom_pp_set(std::vector imovements, bool slew_on); - int past_target = 0; + std::vector point_to_face = {{0, 0, 0}, {0, 0, 0}}; - double SPACING = 0.5; - double LOOK_AHEAD = 7.0; - // bool is_past_target_using_xy = false; + + void odom_path_spacing_set(double spacing); + void odom_path_spacing_set(okapi::QLength p_spacing); + double odom_path_spacing_get(); + + void odom_look_ahead_set(okapi::QLength p_distance); + void odom_look_ahead_set(double distance); + double odom_look_ahead_get(); + void pid_wait_until_index(int index); void pid_wait_until_point(pose target); void pid_wait_until(pose target); - double dlead = 0.5; + void odom_boomerang_dlead_set(double input); + void odom_boomerang_dlead_set(okapi::QLength p_input); double odom_boomerang_dlead_get(); - double max_boomerang_distance = 12.0; + + void odom_boomerang_distance_set(okapi::QLength p_distance); 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; - double track_width = 0.0; - double h_last = 0, v_last = 0 /*, c_last = 0*/; - /*double h = 0, h2 = 0*/; // rad for big circle - double last_theta = 0; - // double Xx = 0, Yy = 0, Xy = 0, Yx = 0; - drive_directions current_drive_direction = fwd; + bool ptf1_running = false; std::vector find_point_to_face(pose current, pose target, drive_directions dir, bool set_global); void raw_pid_odom_ptp_set(odom imovement, bool slew_on); @@ -478,33 +480,22 @@ class Drive { PID aPID; PID internal_leftPID; PID internal_rightPID; - bool was_last_pp_mode_boomerang = false; - bool global_forward_drive_slew_enabled = false; - bool global_backward_drive_slew_enabled = false; void slew_drive_set(bool slew_on); void slew_drive_forward_set(bool slew_on); bool slew_drive_forward_get(); void slew_drive_backward_set(bool slew_on); bool slew_drive_backward_get(); - bool global_forward_swing_slew_enabled = false; - bool global_backward_swing_slew_enabled = false; void slew_swing_set(bool slew_on); void slew_swing_forward_set(bool slew_on); bool slew_swing_forward_get(); void slew_swing_backward_set(bool slew_on); bool slew_swing_backward_get(); - bool global_turn_slew_enabled = false; void slew_turn_set(bool slew_on); bool slew_turn_get(); - e_angle_behavior current_angle_behavior = raw; - - e_angle_behavior default_swing_type = raw; - e_angle_behavior default_turn_type = raw; - e_angle_behavior default_odom_type = shortest; void pid_angle_behavior_set(e_angle_behavior behavior); void pid_turn_behavior_set(e_angle_behavior behavior); void pid_swing_behavior_set(e_angle_behavior behavior); @@ -513,10 +504,10 @@ class Drive { e_angle_behavior pid_swing_behavior_get(); e_angle_behavior pid_odom_behavior_get(); + void pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance); void pid_angle_behavior_tolerance_set(double tolerance); double pid_angle_behavior_tolerance_get(); - double turn_tolerance = 3.0; - bool turn_biased_left = false; + void pid_angle_behavior_bias_set(e_angle_behavior behavior); e_angle_behavior pid_angle_behavior_bias_get(e_angle_behavior); double turn_is_toleranced(double target, double current, double input, double longest, double shortest); @@ -532,16 +523,67 @@ class Drive { tracking_wheel* odom_front_tracker; tracking_wheel* odom_back_tracker; - bool odom_left_tracker_enabled = false; - bool odom_right_tracker_enabled = false; - bool odom_front_tracker_enabled = false; - bool odom_back_tracker_enabled = false; - void odom_tracker_left_set(tracking_wheel* input); void odom_tracker_right_set(tracking_wheel* input); void odom_tracker_front_set(tracking_wheel* input); void odom_tracker_back_set(tracking_wheel* input); + void odom_slew_reenable(bool reenable); // true reenables, false does not + bool odom_slew_reenables(); + + // + // + // odom private + // + // + bool slew_reenables_when_max_speed_changes = true; + int slew_min_when_it_enabled = 0; + bool slew_will_enable_later = false; + bool current_slew_on = false; + bool is_odom_turn_bias_enabled = true; + bool odom_turn_bias_enabled(); + void odom_turn_bias_set(bool set); + double angle_rad = 0.0; + double track_width = 0.0; + bool odometry_enabled = true; + pose odom_target = {0, 0, 0}; + pose odom_current = {0, 0, 0}; + pose odom_second_to_last = {0, 0, 0}; + pose odom_start = {0, 0, 0}; + pose odom_target_start = {0, 0, 0}; + pose turn_to_point_target = {0, 0, 0}; + bool y_flipped = false; + bool x_flipped = false; + double odom_imu_start = 0.0; + int past_target = 0; + double SPACING = 0.5; + double LOOK_AHEAD = 7.0; + double dlead = 0.5; + double max_boomerang_distance = 12.0; + double odom_turn_bias_amount = 1.375; + drive_directions current_drive_direction = fwd; + double h_last = 0, v_last = 0; + double last_theta = 0; + bool was_last_pp_mode_boomerang = false; + bool global_forward_drive_slew_enabled = false; + bool global_backward_drive_slew_enabled = false; + bool global_forward_swing_slew_enabled = false; + bool global_backward_swing_slew_enabled = false; + double turn_tolerance = 3.0; + bool global_turn_slew_enabled = false; + e_angle_behavior current_angle_behavior = raw; + e_angle_behavior default_swing_type = raw; + e_angle_behavior default_turn_type = raw; + e_angle_behavior default_odom_type = shortest; + bool turn_biased_left = false; + std::vector set_odoms_direction(std::vector inputs); + odom set_odom_direction(odom input); + pose flip_pose(pose input); + bool odom_left_tracker_enabled = false; + bool odom_right_tracker_enabled = false; + bool odom_front_tracker_enabled = false; + bool odom_back_tracker_enabled = false; + ///// // // User Control diff --git a/src/EZ-Template/drive/pid_tasks.cpp b/src/EZ-Template/drive/pid_tasks.cpp index c2ac1b0..10af86d 100644 --- a/src/EZ-Template/drive/pid_tasks.cpp +++ b/src/EZ-Template/drive/pid_tasks.cpp @@ -184,7 +184,8 @@ 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)) / odom_turn_bias_amount; + if (is_odom_turn_bias_enabled) + 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); @@ -209,7 +210,7 @@ void Drive::ptp_task() { } // 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("cos %.2f max_slew_out %.2f headingerr: %.2f\n", cos_scale, max_slew_out, aPID.target_get()); + // 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, aPID.output, max_slew_out); // printf("xy(%.2f, %.2f, %.2f) xyPID: %.2f aPID: %.2f dir: %i sgn: %i past_target: %i is_past_target: %i is_past_using_xy: %i fake_xy(%.2f, %.2f, %.2f)\n", odom_current.x, odom_current.y, odom_current.theta, xyPID.target_get(), aPID.target_get(), dir, flipped, past_target, (int)is_past_target(odom_target, odom_current), is_past_target_using_xy, fake_x, fake_y, util::to_deg(fake_angle)); // printf("xy(%.2f, %.2f, %.2f) xyPID: %.2f aPID: %.2f ptf:(%.2f, %.2f)\n", odom_current.x, odom_current.y, odom_current.theta, xyPID.error, aPID.error, ptf.x, ptf.y); @@ -256,6 +257,7 @@ void Drive::pp_task() { if (pp_index < pp_movements.size() - 1) { pp_index = pp_index >= pp_movements.size() - 1 ? pp_index : pp_index + 1; bool slew_on = slew_left.enabled() || slew_right.enabled() ? true : false; + if (!current_slew_on) slew_on = false; raw_pid_odom_ptp_set(pp_movements[pp_index], slew_on); } } diff --git a/src/EZ-Template/drive/set_pid/set_drive_pid.cpp b/src/EZ-Template/drive/set_pid/set_drive_pid.cpp index d4270d4..f1a61be 100644 --- a/src/EZ-Template/drive/set_pid/set_drive_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_drive_pid.cpp @@ -142,6 +142,7 @@ void Drive::pid_drive_set(double target, int speed, bool slew_on, bool toggle_he // Initialize slew slew_left.initialize(slew_on, max_speed, l_target_encoder, drive_sensor_left()); slew_right.initialize(slew_on, max_speed, r_target_encoder, drive_sensor_right()); + current_slew_on = slew_on; // Make sure we're using normal PID leftPID.exit = internal_leftPID.exit; 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 26fcab6..2b7dc6b 100644 --- a/src/EZ-Template/drive/set_pid/set_odom_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_odom_pid.cpp @@ -45,11 +45,55 @@ 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; } +void Drive::odom_boomerang_dlead_set(okapi::QLength p_input) { odom_boomerang_dlead_set(p_input.convert(okapi::inch)); } double Drive::odom_boomerang_dlead_get() { return dlead; } void Drive::odom_boomerang_distance_set(double distance) { max_boomerang_distance = distance; } +void Drive::odom_boomerang_distance_set(okapi::QLength p_distance) { odom_boomerang_distance_set(p_distance.convert(okapi::inch)); } 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; } +void Drive::odom_path_spacing_set(double spacing) { SPACING = spacing; } +void Drive::odom_path_spacing_set(okapi::QLength p_spacing) { odom_path_spacing_set(p_spacing.convert(okapi::inch)); } +double Drive::odom_path_spacing_get() { return SPACING; } +void Drive::odom_look_ahead_set(double distance) { LOOK_AHEAD = distance; } +void Drive::odom_look_ahead_set(okapi::QLength p_distance) { odom_look_ahead_set(p_distance.convert(okapi::inch)); } +double Drive::odom_look_ahead_get() { return LOOK_AHEAD; } +bool Drive::odom_turn_bias_enabled() { return is_odom_turn_bias_enabled; } +void Drive::odom_turn_bias_set(bool set) { is_odom_turn_bias_enabled = set; } +void Drive::odom_slew_reenable(bool reenable) { slew_reenables_when_max_speed_changes = reenable; } +bool Drive::odom_slew_reenables() { return slew_reenables_when_max_speed_changes; } + +///// +// pid_odom_set but it looks like pid_drive_set +///// +void Drive::pid_odom_set(okapi::QLength p_target, int speed, bool slew_on) { + double target = p_target.convert(okapi::inch); + pid_odom_set(target, speed, slew_on); +} +void Drive::pid_odom_set(okapi::QLength p_target, int speed) { + double target = p_target.convert(okapi::inch); + pid_odom_set(target, speed); +} +void Drive::pid_odom_set(double target, int speed) { + bool slew_on = util::sgn(target) >= 0 ? slew_drive_forward_get() : slew_drive_backward_get(); + pid_odom_set(target, speed, slew_on); +} +void Drive::pid_odom_set(double target, int speed, bool slew_on) { + drive_directions fwd_or_rev = util::sgn(target) >= 0 ? fwd : rev; + pose target_pose = util::vector_off_point(target, odom_pose_get()); + odom path = {target_pose, fwd_or_rev, speed}; + + xyPID.timers_reset(); + aPID.timers_reset(); + + if (print_toggle) printf("Injected "); + std::vector input_path = inject_points(set_odoms_direction({path})); + odom_turn_bias_set(false); + current_slew_on = slew_on; + slew_min_when_it_enabled = 0; + slew_will_enable_later = false; + raw_pid_odom_pp_set(input_path, slew_on); +} ///// // pid_odom_set @@ -140,6 +184,10 @@ void Drive::pid_odom_injected_pp_set(std::vector imovements, bool slew if (print_toggle) printf("Injected "); std::vector input_path = inject_points(set_odoms_direction(imovements)); + odom_turn_bias_set(true); + current_slew_on = slew_on; + slew_min_when_it_enabled = 0; + slew_will_enable_later = false; raw_pid_odom_pp_set(input_path, slew_on); } // Units @@ -166,6 +214,10 @@ void Drive::pid_odom_smooth_pp_set(std::vector imovements, bool slew_on) { if (print_toggle) printf("Smooth Injected "); std::vector input_path = smooth_path(inject_points(set_odoms_direction(imovements)), 0.75, 0.03, 0.0001); + odom_turn_bias_set(true); + current_slew_on = slew_on; + slew_min_when_it_enabled = 0; + slew_will_enable_later = false; raw_pid_odom_pp_set(input_path, slew_on); } // Units @@ -242,6 +294,11 @@ void Drive::pid_odom_pp_set(std::vector imovements, bool slew_on) { injected_pp_index.push_back(i); } + odom_turn_bias_set(true); + current_slew_on = slew_on; + slew_min_when_it_enabled = 0; + slew_will_enable_later = false; + if (print_toggle) printf("Pure Pursuit "); raw_pid_odom_pp_set(input, slew_on); } @@ -263,6 +320,10 @@ void Drive::pid_odom_ptp_set(odom imovement, bool slew_on) { l_start = drive_sensor_left(); r_start = drive_sensor_right(); + odom_turn_bias_set(true); + current_slew_on = slew_on; + slew_min_when_it_enabled = 0; + slew_will_enable_later = false; raw_pid_odom_ptp_set(imovement, slew_on); // Initialize slew @@ -321,8 +382,9 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) { current_angle_behavior = imovement.turn_behavior; } - // Set max speed - pid_speed_max_set(imovement.max_xy_speed); + if (current_slew_on && imovement.max_xy_speed > pid_speed_max_get() && odom_slew_reenables()) { + slew_will_enable_later = true; + } // Set targets odom_target.x = imovement.target.x; @@ -344,8 +406,31 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) { // Set constants xyPID.constants_set(pid_consts.kp, pid_consts.ki, pid_consts.kd, pid_consts.start_i); aPID.constants_set(angle_consts.kp, angle_consts.ki, angle_consts.kd, angle_consts.start_i); - slew_left.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed); - slew_right.constants_set(slew_consts.distance_to_travel, slew_consts.min_speed); + + // Set max speed + pid_speed_max_set(imovement.max_xy_speed); + + int slew_min = slew_consts.min_speed; + if (current_slew_on && slew_will_enable_later && !slew_on && odom_slew_reenables()) { + slew_on = true; + slew_will_enable_later = false; + if (slew_min_when_it_enabled > slew_consts.min_speed) + slew_min = slew_min_when_it_enabled; + + slew_left.constants_set(slew_consts.distance_to_travel, slew_min); + slew_right.constants_set(slew_consts.distance_to_travel, slew_min); + + // Initialize slew + int dir = current_drive_direction == REV ? -1 : 1; // If we're going backwards, add a -1 + double dist_to_target = 100.0 * dir; + slew_left.initialize(slew_on, max_speed, dist_to_target + drive_sensor_left(), drive_sensor_left()); + slew_right.initialize(slew_on, max_speed, dist_to_target + drive_sensor_right(), drive_sensor_right()); + } + + if (slew_min_when_it_enabled == 0) { + slew_left.constants_set(slew_consts.distance_to_travel, slew_min); + slew_right.constants_set(slew_consts.distance_to_travel, slew_min); + } bool is_current_boomerang = false; if (mode == PURE_PURSUIT) @@ -361,6 +446,8 @@ void Drive::raw_pid_odom_ptp_set(odom imovement, bool slew_on) { // Get the starting point for if we're positive or negative. This is used to find if we've past target past_target = util::sgn(is_past_target(odom_target, odom_current)); + slew_min_when_it_enabled = pid_speed_max_get(); + // This is used for wait_until int dir = current_drive_direction == REV ? -1 : 1; // If we're going backwards, add a -1 leftPID.target_set(l_start + (LOOK_AHEAD * dir)); diff --git a/src/EZ-Template/drive/set_pid/set_pid.cpp b/src/EZ-Template/drive/set_pid/set_pid.cpp index c5b36af..d668b1b 100644 --- a/src/EZ-Template/drive/set_pid/set_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_pid.cpp @@ -32,6 +32,7 @@ void Drive::pid_angle_behavior_bias_set(e_angle_behavior behavior) { } e_angle_behavior Drive::pid_angle_behavior_bias_get(e_angle_behavior) { return turn_biased_left ? ez::LEFT_TURN : ez::RIGHT_TURN; } void Drive::pid_angle_behavior_tolerance_set(double tolerance) { turn_tolerance = tolerance; } +void Drive::pid_angle_behavior_tolerance_set(okapi::QAngle p_tolerance) { pid_angle_behavior_tolerance_set(p_tolerance.convert(okapi::degree)); } double Drive::pid_angle_behavior_tolerance_get() { return turn_tolerance; } // Changes global default turn behavior to either: diff --git a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp index 56b1e44..3cdf850 100644 --- a/src/EZ-Template/drive/set_pid/set_swing_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_swing_pid.cpp @@ -331,6 +331,7 @@ void Drive::pid_swing_set(e_swing type, double target, int speed, int opposite_s double slew_tar = slew_swing_using_angle ? target : direction * 100; if (!slew_swing_using_angle) slew_tar += current; slew_swing.initialize(slew_on, max_speed, slew_tar, current); + current_slew_on = slew_on; // Run task drive_mode_set(SWING); diff --git a/src/EZ-Template/drive/set_pid/set_turn_pid.cpp b/src/EZ-Template/drive/set_pid/set_turn_pid.cpp index bcdbf12..7471a18 100644 --- a/src/EZ-Template/drive/set_pid/set_turn_pid.cpp +++ b/src/EZ-Template/drive/set_pid/set_turn_pid.cpp @@ -131,6 +131,7 @@ void Drive::pid_turn_set(double target, int speed, e_angle_behavior behavior, bo // Initialize slew slew_turn.initialize(slew_on, max_speed, target, chain_sensor_start); + current_slew_on = slew_on; // Run task drive_mode_set(TURN); diff --git a/src/EZ-Template/drive/tracking.cpp b/src/EZ-Template/drive/tracking.cpp index 2f5e892..f425590 100644 --- a/src/EZ-Template/drive/tracking.cpp +++ b/src/EZ-Template/drive/tracking.cpp @@ -5,6 +5,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. */ #include "EZ-Template/drive/drive.hpp" +#include "EZ-Template/util.hpp" using namespace ez; @@ -12,10 +13,16 @@ using namespace ez; void Drive::odom_x_set(double x) { odom_current.x = x; } void Drive::odom_y_set(double y) { odom_current.y = y; } void Drive::odom_theta_set(double a) { drive_angle_set(a); } +void Drive::odom_x_set(okapi::QLength p_x) { odom_x_set(p_x.convert(okapi::inch)); } +void Drive::odom_y_set(okapi::QLength p_y) { odom_y_set(p_y.convert(okapi::inch)); } +void Drive::odom_theta_set(okapi::QAngle p_a) { odom_theta_set(p_a.convert(okapi::degree)); } void Drive::odom_reset() { odom_pose_set({0, 0, 0}); } void Drive::drive_width_set(double input) { track_width = input; } +void Drive::drive_width_set(okapi::QLength p_input) { drive_width_set(p_input.convert(okapi::inch)); } double Drive::drive_width_get() { return track_width; } void Drive::drive_odom_enable(bool input) { odometry_enabled = input; } +bool Drive::drive_odom_enabled() { return odometry_enabled; } +void Drive::odom_pose_set(united_pose itarget) { odom_pose_set(util::united_pose_to_pose(itarget)); } void Drive::odom_pose_set(pose itarget) { odom_theta_set(itarget.theta); odom_x_set(itarget.x); diff --git a/src/main.cpp b/src/main.cpp index 4884ece..2a8d679 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -6,6 +6,7 @@ ///// // Chassis constructor +/* ez::Drive chassis( // These are your drive motors, the first motor is used for sensing! pros::MotorGroup({-5, -6, -7, -8}), // Left Chassis Ports (negative port will reverse it!) @@ -15,6 +16,15 @@ ez::Drive chassis( 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) 420.0, // Wheel RPM = cartridge * (motor gear / wheel gear) 11.0); // Width of your powered wheels. Measure this with a tape measure, center-to-center +*/ +ez::Drive chassis( + // These are your drive motors, the first motor is used for sensing! + {-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 + 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) + 420.0); // Wheel RPM = cartridge * (motor gear / wheel gear) // Are you using tracking wheels? Comment out which ones you're using here! // `2.75` is the wheel diameter @@ -35,6 +45,8 @@ void initialize() { pros::delay(500); // Stop the user from doing anything while legacy ports configure + chassis.drive_width_set(11_in); + // Are you using tracking wheels? Comment out which ones you're using here! // chassis.odom_tracker_right_set(&right_tracker); // chassis.odom_tracker_left_set(&left_tracker); @@ -108,8 +120,7 @@ void autonomous() { chassis.drive_imu_reset(); // Reset gyro position to 0 chassis.drive_sensor_reset(); // Reset drive sensors to 0 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.odom_pose_set({0_in, 0_in, 0_deg}); chassis.pid_odom_set({{{0_in, 16_in}, fwd, 110}, {{16_in, 16_in}, fwd, 110}},