Version bump 3.2.0-beta.1

EZ-Robotics · Jul 4, 2024 · 4ecc6f5 · 4ecc6f5
1 parent d14f05c
commit 4ecc6f5
Show file tree

Hide file tree

Showing 14 changed files with 142 additions and 37 deletions.
diff --git a/EZ-Template-Example-Project/[email protected] b/EZ-Template-Example-Project/[email protected]
diff --git a/EZ-Template-Example-Project/[email protected] b/EZ-Template-Example-Project/[email protected]
diff --git a/EZ-Template-Example-Project/firmware/EZ-Template.a b/EZ-Template-Example-Project/firmware/EZ-Template.a
diff --git 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<odom> pp_movements;
+  std::vector<int> 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<odom> imovements, bool slew_on = false);
+  void pid_odom_injected_pp_set(std::vector<odom> imovements, bool slew_on = false);
+  void pid_odom_smooth_pp_set(std::vector<odom> imovements, bool slew_on = false);
+  std::vector<odom> smooth_path(std::vector<odom> 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<odom> imovements, bool slew_on);
+  int past_target = 0;
+  std::vector<pose> 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<pose> 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<odom> inject_points(std::vector<odom> 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
@@ -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
@@ -5,28 +5,28 @@
         "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"
                 },
                 "name": "EZ-Template",
                 "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\\[email protected]",

diff --git 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 → [email protected] b/EZ-Template-Example-Project-3.1.0.zip → [email protected]
diff --git a/[email protected] b/[email protected]
diff --git a/[email protected] b/[email protected]
diff --git 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
@@ -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
@@ -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
@@ -1 +1 @@
-3.1.0
+3.2.0-beta.1