diff --git a/EZ-Template-Example-Project/EZ-Template@3.1.0.zip b/EZ-Template-Example-Project/EZ-Template@3.1.0.zip deleted file mode 100644 index 8185d1fb..00000000 Binary files a/EZ-Template-Example-Project/EZ-Template@3.1.0.zip and /dev/null differ diff --git a/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.1.zip b/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.1.zip new file mode 100644 index 00000000..7eed6f03 Binary files /dev/null and b/EZ-Template-Example-Project/EZ-Template@3.2.0-beta.1.zip differ diff --git a/EZ-Template-Example-Project/firmware/EZ-Template.a b/EZ-Template-Example-Project/firmware/EZ-Template.a index 40aa3d77..760fd51e 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/drive/drive.hpp b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp index 582792d2..3deebabc 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/drive/drive.hpp @@ -322,6 +322,72 @@ class Drive { */ void drive_defaults_set(); + ///// + // + // Odometry + // + ///// + + /** + * Tasks for tracking. + */ + // pros::Task ez_tracking; + void ez_tracking_task(); + + void pid_odom_drive_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(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(double input); + double drive_width_get(); + void drive_odom_enable(bool input); + pose odom_target = {0, 0, 0}; + pose odom_current = {0, 0, 0}; + 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_pose_set(pose itarget); + void odom_pose_theta_set(double a); + void odom_reset(); + bool imu_calibration_complete = false; + double angle_rad = 0.0; + void pid_turn_set(pose itarget, turn_types dir, int speed, bool slew_on = false); + pose turn_to_point_target = {0, 0, 0}; + 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); + 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 pid_wait_until_pp(int index); + double dlead = 0.375; + void pid_odom_boomerang_set(odom imovement, bool slew_on = false); + // 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; + turn_types current_drive_direction = fwd; + bool ptf1_running = false; + std::vector find_point_to_face(pose current, pose target, bool set_global = false); + void raw_pid_odom_ptp_set(odom imovement, bool slew_on); + std::vector inject_points(std::vector imovements); + void ptp_task(); + void boomerang_task(); + void pp_task(); + PID xyPID; + PID aPID; + ///// // // User Control diff --git a/EZ-Template-Example-Project/include/EZ-Template/util.hpp b/EZ-Template-Example-Project/include/EZ-Template/util.hpp index 531ab144..c009ff07 100644 --- a/EZ-Template-Example-Project/include/EZ-Template/util.hpp +++ b/EZ-Template-Example-Project/include/EZ-Template/util.hpp @@ -69,7 +69,43 @@ enum exit_output { RUNNING = 1, enum e_mode { DISABLE = 0, SWING = 1, TURN = 2, - DRIVE = 3 }; + TURN_TO_POINT = 3, + DRIVE = 4, + POINT_TO_POINT = 5, + PURE_PURSUIT = 6, + BOOMERANG = 7 }; + +/** + * Enum for turn types + */ +enum turn_types { FWD = 0, + FORWARD = FWD, + fwd = FWD, + forward = FWD, + REV = 1, + REVERSE = REV, + rev = REV, + reverse = REV }; + +const double ANGLE_NOT_SET = 0.0000000000000000000001; + +/** + * Struct for coordinates + */ +typedef struct pose { + double x = 0.0; + double y = 0.0; + double theta = ANGLE_NOT_SET; +} pose; + +/** + * Struct for odom movements + */ +typedef struct odom { + pose target; + turn_types turn_type; + int max_xy_speed; +} odom; /** * Outputs string for exit_condition enum. @@ -103,5 +139,13 @@ const bool SD_CARD_ACTIVE = pros::usd::is_installed(); * Delay time for tasks */ const int DELAY_TIME = 10; + +double to_deg(double input); +double to_rad(double input); +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); + } // namespace util } // namespace ez diff --git a/EZ-Template-Example-Project/project.pros b/EZ-Template-Example-Project/project.pros index a728fdf3..6a59aa50 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.1.0", + "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\EZ-Template@3.2.0-beta.1", "metadata": { "origin": "local" }, @@ -13,20 +13,20 @@ "py/object": "pros.conductor.templates.local_template.LocalTemplate", "supported_kernels": "^4.1.0", "system_files": [ - "include\\EZ-Template\\drive\\drive.hpp", - "include\\EZ-Template\\sdcard.hpp", - "include\\EZ-Template\\auton_selector.hpp", - "include\\EZ-Template\\api.hpp", + "firmware\\EZ-Template.a", "include\\EZ-Template\\PID.hpp", + "include\\EZ-Template\\sdcard.hpp", "include\\EZ-Template\\auton.hpp", + "include\\EZ-Template\\auton_selector.hpp", "include\\EZ-Template\\piston.hpp", - "include\\EZ-Template\\slew.hpp", - "firmware\\EZ-Template.a", - "include\\EZ-Template\\util.hpp" + "include\\EZ-Template\\drive\\drive.hpp", + "include\\EZ-Template\\api.hpp", + "include\\EZ-Template\\util.hpp", + "include\\EZ-Template\\slew.hpp" ], "target": "v5", "user_files": [], - "version": "3.1.0" + "version": "3.2.0-beta.1" }, "kernel": { "location": "C:\\Users\\union\\AppData\\Roaming\\PROS\\templates\\kernel@4.1.0", diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp index 176091e4..718f310e 100644 --- a/EZ-Template-Example-Project/src/main.cpp +++ b/EZ-Template-Example-Project/src/main.cpp @@ -95,8 +95,19 @@ 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(8.93); // just use a tape measure + chassis.dlead = 0.375; + chassis.odometry_enabled = true; - ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector + chassis.pid_odom_smooth_pp_set({{{0, 16, 45}, fwd, 110}, + {{16, 16}, fwd, 110}}); + chassis.pid_wait(); + + chassis.pid_odom_boomerang_set({{0, 0, 0}, rev, 110}); + chassis.pid_wait(); + + // ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector } /** @@ -138,8 +149,8 @@ void opcontrol() { chassis.pid_tuner_iterate(); // Allow PID Tuner to iterate } - chassis.opcontrol_tank(); // Tank control - // chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade + // chassis.opcontrol_tank(); // Tank control + chassis.opcontrol_arcade_standard(ez::SPLIT); // Standard split arcade // chassis.opcontrol_arcade_standard(ez::SINGLE); // Standard single arcade // chassis.opcontrol_arcade_flipped(ez::SPLIT); // Flipped split arcade // chassis.opcontrol_arcade_flipped(ez::SINGLE); // Flipped single arcade diff --git a/EZ-Template-Example-Project-3.1.0.zip b/EZ-Template-Example-Project@3.2.0-beta.1.zip similarity index 81% rename from EZ-Template-Example-Project-3.1.0.zip rename to EZ-Template-Example-Project@3.2.0-beta.1.zip index 1045f9f5..ea84fe06 100644 Binary files a/EZ-Template-Example-Project-3.1.0.zip and b/EZ-Template-Example-Project@3.2.0-beta.1.zip differ diff --git a/EZ-Template@3.1.0.zip b/EZ-Template@3.1.0.zip deleted file mode 100644 index 8185d1fb..00000000 Binary files a/EZ-Template@3.1.0.zip and /dev/null differ diff --git a/EZ-Template@3.2.0-beta.1.zip b/EZ-Template@3.2.0-beta.1.zip new file mode 100644 index 00000000..7eed6f03 Binary files /dev/null and b/EZ-Template@3.2.0-beta.1.zip differ diff --git a/Makefile b/Makefile index aaa9bd78..9582ed6b 100644 --- a/Makefile +++ b/Makefile @@ -27,7 +27,7 @@ EXCLUDE_COLD_LIBRARIES:= IS_LIBRARY:=1 # TODO: CHANGE THIS! LIBNAME:=EZ-Template -VERSION:=3.1.0 +VERSION:=3.2.0-beta.1 # 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 d2ea0e8c..792c27c3 100644 --- a/src/autons.cpp +++ b/src/autons.cpp @@ -14,7 +14,7 @@ const int SWING_SPEED = 90; // Constants /// void default_constants() { - chassis.pid_heading_constants_set(8, 0, 50); // 11, 0, 20 + chassis.pid_heading_constants_set(8, 0, 50); 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); diff --git a/src/main.cpp b/src/main.cpp index 97046e62..96bfb5a0 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,8 +22,6 @@ ez::Drive chassis( * to keep execution time for this mode under a few seconds. */ void initialize() { - chassis.odometry_enabled = false; - // Print our branding over your terminal :D ez::ez_template_print(); @@ -98,29 +96,15 @@ void autonomous() { 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(8.93); + chassis.drive_width_set(8.93); // just use a tape measure chassis.dlead = 0.375; chassis.odometry_enabled = true; - // chassis.pid_drive_set(dist, speed); - // chassis.pid_wait(); - - // chassis.pid_odom_ptp_set({{dist, dist}, fwd, speed}); - // chassis.pid_wait(); - - // chassis.pid_odom_ptp_set({{0, dist}, rev, speed}); - // chassis.pid_wait(); - - // chassis.pid_odom_ptp_set({{0, 0}, fwd, speed}); - // chassis.pid_wait(); + chassis.pid_odom_smooth_pp_set({{{0, 16, 45}, fwd, 110}, + {{16, 16}, fwd, 110}}); + chassis.pid_wait(); - double x = 12; - double y = 16; - int speed = 80; - chassis.pid_odom_smooth_pp_set({{{0, y, -45}, fwd, speed}, - {{x, y}, rev, speed}, - {{x, 0}, fwd, speed}}, - false); + chassis.pid_odom_boomerang_set({{0, 0, 0}, rev, 110}); chassis.pid_wait(); // ez::as::auton_selector.selected_auton_call(); // Calls selected auton from autonomous selector diff --git a/version b/version index a0cd9f0c..4588bb00 100644 --- a/version +++ b/version @@ -1 +1 @@ -3.1.0 \ No newline at end of file +3.2.0-beta.1 \ No newline at end of file