diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.2.zip b/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.2.zip deleted file mode 100644 index b9eed3fd..00000000 Binary files a/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.2.zip and /dev/null differ diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.3.zip b/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.3.zip new file mode 100644 index 00000000..4b476490 Binary files /dev/null and b/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.3.zip differ diff --git a/EZ-Template-Example-Project/firmware/EZ-Template.a b/EZ-Template-Example-Project/firmware/EZ-Template.a index d2c0a57e..b913a1a4 100644 Binary files a/EZ-Template-Example-Project/firmware/EZ-Template.a and b/EZ-Template-Example-Project/firmware/EZ-Template.a differ diff --git a/EZ-Template-Example-Project/include/EZ-Template/PID.hpp b/EZ-Template-Example-Project/include/EZ-Template/PID.hpp index 29605362..062bffe7 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/PID.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/PID.hpp @@ -264,7 +264,7 @@ class PID { private: double velocity_zero_main = 0.05; - double velocity_zero_secondary = 0.1; + double velocity_zero_secondary = 0.075; int i = 0, j = 0, k = 0, l = 0, m = 0; bool is_mA = false; double second_sensor = 0.0; diff --git a/EZ-Template-Example-Project/include/EZ-Template/api.hpp b/EZ-Template-Example-Project/include/EZ-Template/api.hpp index 63bc6aea..7989cf01 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/api.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/api.hpp @@ -13,4 +13,5 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/piston.hpp" #include "EZ-Template/sdcard.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" \ No newline at end of file diff --git a/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp b/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp index 10d0c19a..e8f46feb 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/auton_selector.hpp @@ -17,6 +17,7 @@ class AutonSelector { std::vector Autons; int auton_page_current; int auton_count; + int last_auton_page_current; AutonSelector(); AutonSelector(std::vector autons); void selected_auton_call(); diff --git a/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp index ab29a3fc..896e6748 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp @@ -12,10 +12,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "EZ-Template/PID.hpp" #include "EZ-Template/slew.hpp" +#include "EZ-Template/tracking_wheel.hpp" #include "EZ-Template/util.hpp" #include "okapi/api/units/QAngle.hpp" #include "okapi/api/units/QLength.hpp" #include "okapi/api/units/QTime.hpp" +#include "pros/motor_group.hpp" #include "pros/motors.h" using namespace ez; @@ -233,6 +235,24 @@ class Drive { */ pros::Task ez_auto; + /** + * Creates a Drive Controller using internal encoders and requires track width + * + * \param input_left_motors + * pros::MotorGroup({-1, 2,...}) Make ports negative if reversed! + * \param input_right_motors + * pros::MotorGroup({-3, 4,...}) Make ports negative if reversed! + * \param input_imu + * pros::IMU(21) Port the IMU is plugged into. + * \param wheel_diameter + * Diameter of your drive wheels. Remember 4" is 4.125"! + * \param wheel_rpm + * Motor cartridge RPM + * \param input_track_width + * Distance between the center of your left wheel and the center of your right wheel. You can measure this with a tape measure. + */ + Drive(pros::MotorGroup input_left_motors, pros::MotorGroup input_right_motors, pros::IMU input_imu, double wheel_diameter, double wheel_rpm, double input_track_width); + /** * Creates a Drive Controller using internal encoders. * @@ -338,55 +358,119 @@ 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}; + + void odom_enable(bool input); + bool odom_enabled(); + std::vector pp_movements; std::vector injected_pp_index; int pp_index = 0; - void odom_pose_x_set(double x); - void odom_pose_y_set(double y); + void odom_x_set(double x); + void odom_y_set(double y); + 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_theta_set(double a); + 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, bool slew_on = false); - pose turn_to_point_target = {0, 0, 0}; - void pid_odom_set(odom imovement, bool slew_on = false); - void pid_odom_set(std::vector imovements, bool slew_on = false); - void pid_odom_ptp_set(odom imovement, bool slew_on = false); - void pid_odom_pp_set(std::vector imovements, bool slew_on = false); - void pid_odom_injected_pp_set(std::vector imovements, bool slew_on = false); - void pid_odom_smooth_pp_set(std::vector imovements, bool slew_on = false); + + 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); + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior); + void pid_turn_set(pose itarget, drive_directions dir, int speed, e_angle_behavior behavior, bool slew_on); + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed); + void pid_turn_set(united_pose p_itarget, drive_directions dir, int speed, bool slew_on); + 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); + + 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); + void pid_odom_set(std::vector imovements, bool slew_on); + void pid_odom_set(united_odom p_imovement); + void pid_odom_set(united_odom p_imovement, bool slew_on); + void pid_odom_set(std::vector p_imovements); + void pid_odom_set(std::vector p_imovements, bool slew_on); + + void pid_odom_pp_set(std::vector imovements); + void pid_odom_pp_set(std::vector imovements, bool slew_on); + void pid_odom_pp_set(std::vector p_imovements); + void pid_odom_pp_set(std::vector p_imovements, bool slew_on); + + void pid_odom_injected_pp_set(std::vector imovements); + void pid_odom_injected_pp_set(std::vector imovements, bool slew_on); + void pid_odom_injected_pp_set(std::vector p_imovements); + void pid_odom_injected_pp_set(std::vector p_imovements, bool slew_on); + + void pid_odom_smooth_pp_set(std::vector imovements); + void pid_odom_smooth_pp_set(std::vector imovements, bool slew_on); + void pid_odom_smooth_pp_set(std::vector p_imovements); + void pid_odom_smooth_pp_set(std::vector p_imovements, bool slew_on); + + void pid_odom_boomerang_set(odom imovement); + void pid_odom_boomerang_set(odom imovement, bool slew_on); + void pid_odom_boomerang_set(united_odom p_imovement); + void pid_odom_boomerang_set(united_odom p_imovement, bool slew_on); + + // 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(); + 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.375; - void pid_odom_boomerang_set(odom imovement, bool slew_on = false); + + void odom_boomerang_dlead_set(double input); + void odom_boomerang_dlead_set(okapi::QLength p_input); + double odom_boomerang_dlead_get(); + + void odom_boomerang_distance_set(okapi::QLength p_distance); + void odom_boomerang_distance_set(double distance); + double odom_boomerang_distance_get(); + + void odom_turn_bias_set(double bias); + double odom_turn_bias_get(); + // Odometry - bool odometry_enabled = true; - float track_width = 0.0; - double l_last = 0, r_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, bool set_global = 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); std::vector inject_points(std::vector imovements); void ptp_task(); @@ -396,7 +480,109 @@ class Drive { PID aPID; PID internal_leftPID; PID internal_rightPID; + + 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(); + + 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(); + + void slew_turn_set(bool slew_on); + bool slew_turn_get(); + + 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); + void pid_odom_behavior_set(e_angle_behavior behavior); + e_angle_behavior pid_turn_behavior_get(); + 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(); + + 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); + double turn_short(double target, double current, bool print = false); + double turn_long(double target, double current, bool print = false); + double new_turn_target_compute(double target, double current, ez::e_angle_behavior behavior); + double turn_left(double target, double current, bool print = false); + double turn_right(double target, double current, bool print = false); + bool is_swing_slew_enabled(e_swing type, double target, double current); + + tracking_wheel* odom_left_tracker; + tracking_wheel* odom_right_tracker; + tracking_wheel* odom_front_tracker; + tracking_wheel* odom_back_tracker; + + 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; ///// // @@ -791,7 +977,17 @@ class Drive { ///// /** - * Sets the robot to move forward using PID with okapi units. + * Sets the robot to move forward using PID with okapi units, only using slew if globally enabled. + * + * \param target + * target value in inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(okapi::QLength p_target, int speed); + + /** + * Sets the robot to move forward using PID with okapi units, using slew if enabled for this motion. * * \param target * target value in inches @@ -800,12 +996,22 @@ class Drive { * \param slew_on * ramp up from a lower speed to your target speed * \param toggle_heading - * toggle for heading correction + * toggle for heading correction. true enables, false disables */ - void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_drive_set(okapi::QLength p_target, int speed, bool slew_on, bool toggle_heading = true); /** - * Sets the robot to move forward using PID without okapi units. + * Sets the robot to move forward using PID without okapi units, only using slew if globally enabled. + * + * \param target + * target value as a double, unit is inches + * \param speed + * 0 to 127, max speed during motion + */ + void pid_drive_set(double target, int speed); + + /** + * Sets the robot to move forward using PID without okapi units, using slew if enabled for this motion. * * \param target * target value as a double, unit is inches @@ -814,9 +1020,21 @@ class Drive { * \param slew_on * ramp up from a lower speed to your target speed * \param toggle_heading - * toggle for heading correction + * toggle for heading correction. true enables, false disables + */ + void pid_drive_set(double target, int speed, bool slew_on, bool toggle_heading = true); + + /** + * Sets the robot to turn using PID. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed */ - void pid_drive_set(double target, int speed, bool slew_on = false, bool toggle_heading = true); + void pid_turn_set(double target, int speed); /** * Sets the robot to turn using PID. @@ -825,10 +1043,46 @@ class Drive { * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(double target, int speed, bool slew_on); + + /** + * Sets the robot to turn using PID, using slew if enabled for this motion. + * + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw * \param slew_on * ramp up from a lower speed to your target speed */ - void pid_turn_set(double target, int speed, bool slew_on = false); + void pid_turn_set(double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn using PID with okapi units. + * + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_set(okapi::QAngle p_target, int speed); /** * Sets the robot to turn using PID with okapi units. @@ -837,51 +1091,253 @@ class Drive { * target value in degrees * \param speed * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion * \param slew_on * ramp up from a lower speed to your target speed */ - void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_turn_set(okapi::QAngle p_target, int speed, bool slew_on); /** - * Sets the robot to turn relative to current heading using PID with okapi units. + * Sets the robot to turn using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. * * \param p_target * target value in okapi angle units * \param speed * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + * \param slew_on + * ramp up from a lower speed to your target speed + */ + void pid_turn_relative_set(okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_turn_relative_set(double target, int speed); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw + */ + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion * \param slew_on * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(okapi::QAngle p_target, int speed, bool slew_on = false); + void pid_turn_relative_set(double target, int speed, bool slew_on); /** - * Sets the robot to turn relative to current heading using PID without okapi units. + * Sets the robot to turn relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param p_target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion + * \param behavior + * changes what direction the robot will turn. can be left, right, shortest, longest, raw * \param slew_on * ramp up from a lower speed to your target speed */ - void pid_turn_relative_set(double target, int speed, bool slew_on = false); + void pid_turn_relative_set(double target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Turn using only the left or right side without okapi units. + * Turn using only the left or right side without okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING - * \param p_target + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed); + + /** + * Turn using only the left or right side without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior); + + /** + * Turn using only the left or right side without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, bool slew_on); + + /** + * Turn using only the left or right side without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Turn using only the left or right side without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed); + + /** + * Turn using only the left or right side without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Turn using only the left or right side without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); + + /** + * Turn using only the left or right side without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param target * target value as a double, unit is degrees * \param speed * 0 to 127, max speed during motion * \param opposite_speed * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. */ - void pid_swing_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); + + /** + * Turn using only the left or right side with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed); /** - * Turn using only the left or right side with okapi units. + * Turn using only the left or right side with okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING @@ -892,10 +1348,138 @@ class Drive { * \param opposite_speed * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. */ - void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Turn using only the left or right side with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Turn using only the left or right side with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Turn using only the left or right side with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); + + /** + * Turn using only the left or right side with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Turn using only the left or right side with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); + + /** + * Turn using only the left or right side with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, e_angle_behavior behavior, bool slew_on); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. * * \param type * L_SWING or R_SWING @@ -906,10 +1490,128 @@ class Drive { * \param opposite_speed * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. */ - void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed); /** - * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units. + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID with okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value in okapi angle units + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, okapi::QAngle p_target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, double target, int speed); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, double target, int speed, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + */ + void pid_swing_relative_set(e_swing type, double target, int speed, e_angle_behavior behavior, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, only using slew if globally enabled. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. * * \param type * L_SWING or R_SWING @@ -920,7 +1622,21 @@ class Drive { * \param opposite_speed * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. */ - void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed = 0, bool slew_on = false); + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, bool slew_on); + + /** + * Sets the robot to turn only using the left or right side relative to current heading using PID without okapi units, using slew if enabled for this motion. + * + * \param type + * L_SWING or R_SWING + * \param p_target + * target value as a double, unit is degrees + * \param speed + * 0 to 127, max speed during motion + * \param opposite_speed + * -127 to 127, max speed of the opposite side of the drive during the swing. This is used for arcs, and is defaulted to 0. + */ + void pid_swing_relative_set(e_swing type, double target, int speed, int opposite_speed, e_angle_behavior behavior, bool slew_on); /** * Resets all PID targets to 0. @@ -1581,6 +2297,22 @@ class Drive { */ double pid_tuner_increment_start_i_get(); + struct const_and_name { + std::string name = ""; + PID::Constants *consts; + }; + + /** + * Vector used for PID Tuner + */ + std::vector pid_tuner_pids = { + {"Drive Forward PID Constants", &forward_drivePID.constants}, + {"Drive Backward PID Constants", &backward_drivePID.constants}, + {"Heading PID Constants", &headingPID.constants}, + {"Turn PID Constants", &turnPID.constants}, + {"Swing Forward PID Constants", &forward_swingPID.constants}, + {"Swing Backward PID Constants", &backward_swingPID.constants}}; + private: // !Auton double chain_target_start = 0.0; double chain_sensor_start = 0.0; @@ -1605,11 +2337,6 @@ class Drive { bool slew_swing_using_angle = false; bool pid_tuner_terminal_b = false; bool pid_tuner_lcd_b = true; - struct const_and_name { - std::string name = ""; - PID::Constants *consts; - }; - std::vector constants; void pid_tuner_print(); void pid_tuner_value_modify(float p, float i, float d, float start); void pid_tuner_value_increase(); @@ -1700,6 +2427,7 @@ class Drive { #define DRIVE_INTEGRATED 1 #define DRIVE_ADI_ENCODER 2 #define DRIVE_ROTATION 3 +#define ODOM_TRACKER 4 /** * Is tracking? @@ -1732,7 +2460,7 @@ class Drive { /** * Function for button presses. */ - void button_press(button_ *input_name, int button, std::function changeCurve, std::function save); + void button_press(button_* input_name, int button, std::function changeCurve, std::function save); /** * The left and right curve scalers. diff --git a/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp b/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp index ea8d5427..85ea9951 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/sdcard.hpp @@ -12,6 +12,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. namespace ez { namespace as { extern AutonSelector auton_selector; + /** * Sets sd card to current page. */ @@ -53,8 +54,9 @@ extern bool turn_off; extern pros::adi::DigitalIn* limit_switch_left; extern pros::adi::DigitalIn* limit_switch_right; + /** - * Initialize two limitswithces to change pages on the lcd + * Initialize two limit switches to change pages on the lcd * * @param left_limit_port * port for the left limit switch @@ -67,5 +69,9 @@ void limit_switch_lcd_initialize(pros::adi::DigitalIn* right_limit, pros::adi::D * pre_auto_task */ void limitSwitchTask(); + +int page_blank_current(); +bool page_blank_is_on(int page); +extern int amount_of_blank_pages; } // namespace as } // namespace ez diff --git a/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp b/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp new file mode 100644 index 00000000..cfab1d44 --- /dev/null +++ b/EZ-Template-Example-Project/include/EZ-Template/tracking_wheel.hpp @@ -0,0 +1,160 @@ +/* +This Source Code Form is subject to the terms of the Mozilla Public +License, v. 2.0. If a copy of the MPL was not distributed with this +file, You can obtain one at http://mozilla.org/MPL/2.0/. +*/ + +#pragma once + +#include "pros/adi.hpp" +#include "pros/rotation.hpp" + +namespace ez { +class tracking_wheel { + public: + pros::adi::Encoder adi_encoder; + pros::Rotation smart_encoder; + + /** + * Creates a new tracking wheel with an ADI Encoder. + * + * \param ports + * {'A', 'B'}. Make the encoder negative if reversed! + * \param wheel_diameter + * Assumed inches, this is the diameter of your wheel. + * \param distance_to_center + * The distance to the center of your robot. this is used for tracking. + * \param ratio + * The gear ratio of your tracking wheel. If it's not geared, this should be 1.0. + */ + tracking_wheel(std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with an ADI Encoder plugged into a 3-wire expander. + * + * \param smart_port + * The smart port your ADI Expander is plugged into. + * \param ports + * {'A', 'B'}. Make the encoder negative if reversed! + * \param wheel_diameter + * Assumed inches, this is the diameter of your wheel. + * \param distance_to_center + * The distance to the center of your robot. this is used for tracking. + * \param ratio + * The gear ratio of your tracking wheel. If it's not geared, this should be 1.0. + */ + tracking_wheel(int smart_port, std::vector ports, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Creates a new tracking wheel with a Rotation sensor. + * + * \param port + * The port your Rotation sensor is plugged into. + * \param wheel_diameter + * Assumed inches, this is the diameter of your wheel. + * \param distance_to_center + * The distance to the center of your robot. this is used for tracking. + * \param ratio + * The gear ratio of your tracking wheel. If it's not geared, this should be 1.0. + */ + tracking_wheel(int port, double wheel_diameter, double distance_to_center = 0.0, double ratio = 1.0); + + /** + * Returns how far the wheel has traveled in inches. + */ + double get(); + + /** + * Returns the raw sensor value. + */ + double get_raw(); + + /** + * Sets the distance to the center of the robot. + * + * \param input + * Distance to the center of your robot in inches. + */ + void distance_to_center_set(double input); + + /** + * Returns the distance to the center of the robot. + */ + double distance_to_center_get(); + + /** + * Sets the distance to the center to be flipped to negative or not. + * + * \param input + * flips distance to center to negative. false leaves it alone, true flips it. + */ + void distance_to_center_flip_set(bool input); + + /** + * Returns if the distance to center is flipped or not. False is not, true is. + */ + bool distance_to_center_flip_get(); + + /** + * Resets your sensor. + */ + void reset(); + + /** + * Returns the constant for how many ticks is 1 inch. + */ + double ticks_per_inch(); + + /** + * Sets the amount of ticks per revolution of your sensor. This is useful for custom encoders. + * + * \param input + * Ticks per revolution + */ + void ticks_per_rev_set(double input); + + /** + * Returns the amount of ticks per revolution of your sensor. + */ + double ticks_per_rev_get(); + + /** + * Sets the gear ratio for your tracking wheel. + * + * \param input + * Gear ratio of tracking wheel + */ + void ratio_set(double input); + + /** + * Returns the gear ratio for your tracking wheel. + */ + double ratio_get(); + + /** + * Sets the diameter of your wheel. + * + * \param input + * Wheel diameter + */ + void wheel_diameter_set(double input); + + /** + * Returns the diameter of your wheel. + */ + double wheel_diameter_get(); + + private: +#define DRIVE_ADI_ENCODER 2 +#define DRIVE_ROTATION 3 + int IS_TRACKER = 0; + + bool IS_FLIPPED = false; + + double DISTANCE_TO_CENTER = 0.0; + double WHEEL_DIAMETER = 0.0; + double RATIO = 1.0; + double ENCODER_TICKS_PER_REV = 0.0; + double WHEEL_TICK_PER_REV = 0.0; +}; +}; // namespace ez \ No newline at end of file diff --git a/EZ-Template-Example-Project/include/EZ-Template/util.hpp b/EZ-Template-Example-Project/include/EZ-Template/util.hpp index 84853b1f..968c7e61 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/util.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/util.hpp @@ -11,6 +11,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. #include #include "api.h" +#include "okapi/api/units/QAngle.hpp" +#include "okapi/api/units/QLength.hpp" +#include "okapi/api/units/QTime.hpp" +#include "util.hpp" + +using namespace okapi::literals; /** * Controller. @@ -75,28 +81,53 @@ enum e_mode { DISABLE = 0, PURE_PURSUIT = 6 }; /** - * Enum for turn types + * Enum for drive directions */ enum drive_directions { FWD = 0, - FORWARD = FWD, - fwd = FWD, - forward = FWD, - REV = 1, - REVERSE = REV, - rev = REV, - reverse = REV }; + FORWARD = FWD, + fwd = FWD, + forward = FWD, + REV = 1, + REVERSE = REV, + rev = REV, + reverse = REV }; + +/** + * Enum for turn types + */ +enum e_angle_behavior { raw = 0, + left_turn = 1, + LEFT_TURN = 1, + counterclockwise = 1, + ccw = 1, + right_turn = 2, + RIGHT_TURN = 2, + clockwise = 2, + cw = 2, + shortest = 3, + longest = 4 }; const double ANGLE_NOT_SET = 0.0000000000000000000001; +const okapi::QAngle p_ANGLE_NOT_SET = 0.0000000000000000000001_deg; /** * Struct for coordinates */ typedef struct pose { - double x = 0.0; - double y = 0.0; + double x; + double y; double theta = ANGLE_NOT_SET; } pose; +/** + * Struct for united coordinates + */ +typedef struct united_pose { + okapi::QLength x; + okapi::QLength y; + okapi::QAngle theta = p_ANGLE_NOT_SET; +} united_pose; + /** * Struct for odom movements */ @@ -104,8 +135,19 @@ typedef struct odom { pose target; drive_directions drive_direction; int max_xy_speed; + e_angle_behavior turn_behavior = shortest; } odom; +/** + * Struct for united odom movements + */ +typedef struct united_odom { + united_pose target; + drive_directions drive_direction; + int max_xy_speed; + e_angle_behavior turn_behavior = shortest; +} united_odom; + /** * Outputs string for exit_condition enum. */ @@ -129,6 +171,11 @@ bool reversed_active(double input); */ double clamp(double input, double max, double min); +/** + * Returns input restricted to min-max threshold. The minimum used is -max + */ +double clamp(double input, double max); + /** * Is the SD card plugged in? */ @@ -145,6 +192,11 @@ double absolute_angle_to_point(pose itarget, pose icurrent); double wrap_angle(double theta); double distance_to_point(pose itarget, pose icurrent); pose vector_off_point(double added, pose icurrent); +double turn_shortest(double target, double current, bool print = false); +double turn_longest(double target, double current, bool print = false); +pose united_pose_to_pose(united_pose input); +std::vector united_odoms_to_odoms(std::vector inputs); +odom united_odom_to_odom(united_odom input); } // namespace util } // namespace ez diff --git a/EZ-Template-Example-Project/project.pros b/EZ-Template-Example-Project/project.pros index acd3cbd1..10eff17a 100644 --- a/EZ-Template-Example-Project/project.pros +++ b/EZ-Template-Example-Project/project.pros @@ -5,7 +5,7 @@ "target": "v5", "templates": { "EZ-Template": { - "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.0-beta.2", + "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.0-beta.3", "metadata": { "origin": "local" }, @@ -13,20 +13,21 @@ "py/object": "pros.conductor.templates.local_template.LocalTemplate", "supported_kernels": "^4.1.0", "system_files": [ + "firmware\\EZ-Template.a", "include\\EZ-Template\\util.hpp", - "include\\EZ-Template\\drive\\drive.hpp", "include\\EZ-Template\\auton_selector.hpp", "include\\EZ-Template\\piston.hpp", - "include\\EZ-Template\\api.hpp", - "include\\EZ-Template\\PID.hpp", + "include\\EZ-Template\\sdcard.hpp", + "include\\EZ-Template\\tracking_wheel.hpp", "include\\EZ-Template\\auton.hpp", + "include\\EZ-Template\\PID.hpp", + "include\\EZ-Template\\drive\\drive.hpp", "include\\EZ-Template\\slew.hpp", - "firmware\\EZ-Template.a", - "include\\EZ-Template\\sdcard.hpp" + "include\\EZ-Template\\api.hpp" ], "target": "v5", "user_files": [], - "version": "3.2.0-beta.2" + "version": "3.2.0-beta.3" }, "kernel": { "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.0", diff --git a/EZ-Template-Example-Project/src/autons.cpp b/EZ-Template-Example-Project/src/autons.cpp index 17518f53..e21eb84f 100644 --- a/EZ-Template-Example-Project/src/autons.cpp +++ b/EZ-Template-Example-Project/src/autons.cpp @@ -8,30 +8,43 @@ // These are out of 127 const int DRIVE_SPEED = 110; const int TURN_SPEED = 90; -const int SWING_SPEED = 90; +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); + + // The amount that turns are prioritized over driving in odom motions + // - this is fully disabled for straight motions + // - if you have tracking wheels, you can run this lower + chassis.odom_turn_bias_set(1.375); + + // Defaults the turn behavior to always go the shortest way + chassis.pid_angle_behavior_set(ez::shortest); } /// diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp index 35e0309b..b093f327 100644 --- a/EZ-Template-Example-Project/src/main.cpp +++ b/EZ-Template-Example-Project/src/main.cpp @@ -15,6 +15,13 @@ ez::Drive chassis( 4.125, // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!) 343); // Wheel RPM +// Are you using tracking wheels? Comment out which ones you're using here! +// `2.75` is the wheel diameter +// `4.0` is the distance from the center of the wheel to the center of the robot +// ez::tracking_wheel right_tracker({-'A', -'B'}, 2.75, 4.0); // ADI Encoders +// ez::tracking_wheel left_tracker(1, {'C', 'D'}, 2.75, 4.0); // ADI Encoders plugged into a Smart port +// ez::tracking_wheel horiz_tracker(1, 2.75, 4.0); // Rotation sensors + /** * Runs initialization code. This occurs as soon as the program is started. * @@ -27,6 +34,11 @@ void initialize() { pros::delay(500); // Stop the user from doing anything while legacy ports configure + // 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); + // chassis.odom_tracker_back_set(&horiz_tracker); + // Configure your chassis controls chassis.opcontrol_curve_buttons_toggle(true); // Enables modifying the controller curve with buttons on the joysticks chassis.opcontrol_drive_activebrake_set(0); // Sets the active brake kP. We recommend ~2. 0 will disable. @@ -95,30 +107,82 @@ 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.dlead = 0.5; - chassis.odometry_enabled = true; - - /* - chassis.pid_odom_set({{{0, 16}, fwd, 110}, - {{16, 16}, fwd, 110}}, + chassis.odom_pose_set({0_in, 0_in, 0_deg}); + chassis.drive_width_set(11_in); // Measure this with a tape measure + + chassis.pid_odom_set({{{0_in, 16_in}, fwd, 110}, + {{16_in, 16_in}, fwd, 110}}, true); chassis.pid_wait(); - chassis.pid_odom_set({{0, 0, 0}, rev, 110}, true); + chassis.pid_odom_set({{0_in, 0_in, 0_deg}, rev, 110}, true); chassis.pid_wait(); - */ - chassis.pid_odom_smooth_pp_set({{{0, 16}, fwd, 110}, - {{16, 16}, fwd, 110}}, - true); - chassis.pid_wait(); + // ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector +} - chassis.pid_odom_boomerang_set({{0, 0, 0}, rev, 110}, true); - chassis.pid_wait(); +/** + * Gives you some extras to run in your opcontrol: + * - run your autonomous routine in opcontrol by pressing DOWN and B + * - to prevent this from accidentally happening at a competition, this + * is only enabled when you're not connected to competition control. + * - gives you a GUI to change your PID values live by pressing X + */ +void ez_template_etxras() { + if (!pros::competition::is_connected()) { + // PID Tuner + // - after you find values that you're happy with, you'll have to set them in auton.cpp + + // Enable / Disable PID Tuner + // When enabled: + // * use A and Y to increment / decrement the constants + // * use the arrow keys to navigate the constants + if (master.get_digital_new_press(DIGITAL_X)) + chassis.pid_tuner_toggle(); + + // Trigger the selected autonomous routine + if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { + pros::motor_brake_mode_e_t preference = chassis.drive_brake_get(); + autonomous(); + chassis.drive_brake_set(preference); + } - // ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector + // Blank pages for Odom Debugging + if (chassis.odom_enabled()) { + // This is Blank Page 1, it will display X, Y, and Angle + if (ez::as::page_blank_is_on(0)) { + screen_print("x: " + std::to_string(chassis.odom_x_get()) + + "\ny: " + std::to_string(chassis.odom_y_get()) + + "\nangle: " + std::to_string(chassis.odom_theta_get()), + 1); // Don't override the top Page line + } + // This is Blank Page 2, it will display every tracking wheel. + // Make sure the tracking wheels read POSITIVE going forwards or left. + else if (ez::as::page_blank_is_on(1)) { + if (chassis.odom_left_tracker != nullptr) + screen_print("left tracker: " + std::to_string(chassis.odom_left_tracker->get()), 1); + else + screen_print("no left tracker", 1); + + if (chassis.odom_right_tracker != nullptr) + screen_print("right tracker: " + std::to_string(chassis.odom_right_tracker->get()), 2); + else + screen_print("no right tracker", 2); + + if (chassis.odom_back_tracker != nullptr) + screen_print("back tracker: " + std::to_string(chassis.odom_back_tracker->get()), 3); + else + screen_print("no back tracker", 3); + + if (chassis.odom_front_tracker != nullptr) + screen_print("front tracker: " + std::to_string(chassis.odom_front_tracker->get()), 4); + else + screen_print("no front tracker", 4); + } + } + + chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate + } } /** @@ -136,29 +200,11 @@ void autonomous() { */ void opcontrol() { // This is preference to what you like to drive on - pros::motor_brake_mode_e_t driver_preference_brake = MOTOR_BRAKE_COAST; - - chassis.drive_brake_set(driver_preference_brake); + chassis.drive_brake_set(MOTOR_BRAKE_COAST); while (true) { - // PID Tuner - // After you find values that you're happy with, you'll have to set them in auton.cpp - if (!pros::competition::is_connected()) { - // Enable / Disable PID Tuner - // When enabled: - // * use A and Y to increment / decrement the constants - // * use the arrow keys to navigate the constants - if (master.get_digital_new_press(DIGITAL_X)) - chassis.pid_tuner_toggle(); - - // Trigger the selected autonomous routine - if (master.get_digital(DIGITAL_B) && master.get_digital(DIGITAL_DOWN)) { - autonomous(); - chassis.drive_brake_set(driver_preference_brake); - } - - chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate - } + // Gives you some extras to make EZ-Template easier + ez_template_etxras(); chassis.opcontrol_tank(); // Tank control // chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade @@ -172,4 +218,4 @@ void opcontrol() { pros::delay(ez::util::DELAY_TIME); // This is used for timer calculations! Keep this ez::util::DELAY_TIME } -} \ No newline at end of file +} diff --git a/EZ-Template-Example-Project@3.2.0-beta.2.zip b/EZ-Template-Example-Project@3.2.0-beta.3.zip similarity index 78% rename from EZ-Template-Example-Project@3.2.0-beta.2.zip rename to EZ-Template-Example-Project@3.2.0-beta.3.zip index ee0b3f6a..ccb19842 100644 Binary files a/EZ-Template-Example-Project@3.2.0-beta.2.zip and b/EZ-Template-Example-Project@3.2.0-beta.3.zip differ diff --git a/EZ-Template@3.2.0-beta.2.zip b/EZ-Template@3.2.0-beta.2.zip deleted file mode 100644 index b9eed3fd..00000000 Binary files a/EZ-Template@3.2.0-beta.2.zip and /dev/null differ diff --git a/EZ-Template@3.2.0-beta.3.zip b/EZ-Template@3.2.0-beta.3.zip new file mode 100644 index 00000000..4b476490 Binary files /dev/null and b/EZ-Template@3.2.0-beta.3.zip differ diff --git a/Makefile b/Makefile index e8082a10..15234876 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:= IS_LIBRARY:=1 # TODO: CHANGE THIS! LIBNAME:=EZ-Template -VERSION:=3.2.0-beta.2 +VERSION:=3.2.0-beta.3 # EXCLUDE_SRC_FROM_LIB= $(SRCDIR)/unpublishedfile.c # this line excludes opcontrol.c and similar files EXCLUDE_SRC_FROM_LIB+=$(foreach file, $(SRCDIR)/autons $(SRCDIR)/main,$(foreach cext,$(CEXTS),$(file).$(cext)) $(foreach cxxext,$(CXXEXTS),$(file).$(cxxext))) diff --git a/src/autons.cpp b/src/autons.cpp index 2530eb2c..e21eb84f 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -38,6 +38,11 @@ void default_constants() { chassis.slew_drive_constants_set(3_in, 70); chassis.slew_swing_constants_set(3_in, 80); + // The amount that turns are prioritized over driving in odom motions + // - this is fully disabled for straight motions + // - if you have tracking wheels, you can run this lower + chassis.odom_turn_bias_set(1.375); + // Defaults the turn behavior to always go the shortest way chassis.pid_angle_behavior_set(ez::shortest); } diff --git a/version b/version index 2d452fe9..1758d11c 100644 --- a/version +++ b/version @@ -1 +1 @@ -3.2.0-beta.2 \ No newline at end of file +3.2.0-beta.3 \ No newline at end of file