Merge pull request #214 from EZ-Robotics/feature/xyt-set

Added xyt set
EZ-Robotics · Dec 4, 2024 · 1de5b28 · 1de5b28
2 parents 5954ae0 + 9c3f7df
commit 1de5b28
Show file tree

Hide file tree

Showing 4 changed files with 45 additions and 15 deletions.
diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp
@@ -103,11 +103,11 @@ void competition_initialize() {
  * from where it left off.
  */
 void autonomous() {
-  chassis.pid_targets_reset();                 // Resets PID targets to 0
-  chassis.drive_imu_reset();                   // Reset gyro position to 0
-  chassis.drive_sensor_reset();                // Reset drive sensors to 0
-  chassis.odom_pose_set({0_in, 0_in, 0_deg});  // Set the current position, you can start at a specific position with this
-  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);   // Set motors to hold.  This helps autonomous consistency
+  chassis.pid_targets_reset();                // Resets PID targets to 0
+  chassis.drive_imu_reset();                  // Reset gyro position to 0
+  chassis.drive_sensor_reset();               // Reset drive sensors to 0
+  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
+  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency
 
   /*
   Odometry and Pure Pursuit are not magic

diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp
@@ -575,21 +575,45 @@ class Drive {
    *
    * \param x
    *        new x value, in inches
-   * \param x
+   * \param y
    *        new y value, in inches
    */
   void odom_xy_set(double x, double y);
 
   /**
    * Sets a new X and Y value for the robot
    *
-   * \param x
+   * \param p_x
    *        new x value, okapi unit
-   * \param x
+   * \param p_y
    *        new y value, okapi unit
    */
   void odom_xy_set(okapi::QLength p_x, okapi::QLength p_y);
 
+  /**
+   * Sets a new X, Y, and Theta value for the robot
+   *
+   * \param x
+   *        new x value, in inches
+   * \param y
+   *        new y value, in inches
+   * \param t
+   *        new theta value, in degrees
+   */
+  void odom_xyt_set(double x, double y, double t);
+
+  /**
+   * Sets a new X, Y, and Theta value for the robot
+   *
+   * \param p_x
+   *        new x value, okapi unit
+   * \param p_y
+   *        new y value, okapi unit
+   * \param p_t
+   *        new theta value, okapi unit
+   */
+  void odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t);
+
   /**
    * Returns the current pose of the robot
    */

diff --git a/src/EZ-Template/drive/tracking.cpp b/src/EZ-Template/drive/tracking.cpp
@@ -44,10 +44,16 @@ void Drive::odom_xy_set(double x, double y) {
   odom_y_set(y);
 }
 void Drive::odom_xy_set(okapi::QLength p_x, okapi::QLength p_y) { odom_xy_set(p_x.convert(okapi::inch), p_y.convert(okapi::inch)); }
+void Drive::odom_xyt_set(double x, double y, double t) {
+  odom_x_set(x);
+  odom_y_set(y);
+  odom_theta_set(t);
+}
+void Drive::odom_xyt_set(okapi::QLength p_x, okapi::QLength p_y, okapi::QAngle p_t) { odom_xyt_set(p_x.convert(okapi::inch), p_y.convert(okapi::inch), p_t.convert(okapi::degree)); }
 void Drive::odom_pose_set(pose itarget) {
-  odom_theta_set(itarget.theta);
   odom_x_set(itarget.x);
   odom_y_set(itarget.y);
+  odom_theta_set(itarget.theta);
 }
 void Drive::odom_pose_set(united_pose itarget) { odom_pose_set(util::united_pose_to_pose(itarget)); }
 void Drive::odom_reset() { odom_pose_set({0.0, 0.0, 0.0}); }
@@ -60,7 +66,7 @@ double Drive::odom_theta_get() { return odom_current.theta; }
 pose Drive::odom_pose_get() { return odom_current; }
 double Drive::drive_width_get() { return global_track_width; }
 
-std::pair<float, float> Drive::decide_vert_sensor(ez::tracking_wheel *tracker, bool is_tracker_enabled, float ime, float ime_track) {
+std::pair<float, float> Drive::decide_vert_sensor(ez::tracking_wheel* tracker, bool is_tracker_enabled, float ime, float ime_track) {
   float current = ime;
   float track_width = ime_track;
   if (is_tracker_enabled) {

diff --git a/src/main.cpp b/src/main.cpp
@@ -103,11 +103,11 @@ void competition_initialize() {
  * from where it left off.
  */
 void autonomous() {
-  chassis.pid_targets_reset();                 // Resets PID targets to 0
-  chassis.drive_imu_reset();                   // Reset gyro position to 0
-  chassis.drive_sensor_reset();                // Reset drive sensors to 0
-  chassis.odom_pose_set({0_in, 0_in, 0_deg});  // Set the current position, you can start at a specific position with this
-  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);   // Set motors to hold.  This helps autonomous consistency
+  chassis.pid_targets_reset();                // Resets PID targets to 0
+  chassis.drive_imu_reset();                  // Reset gyro position to 0
+  chassis.drive_sensor_reset();               // Reset drive sensors to 0
+  chassis.odom_xyt_set(0_in, 0_in, 0_deg);    // Set the current position, you can start at a specific position with this
+  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);  // Set motors to hold.  This helps autonomous consistency
 
   /*
   Odometry and Pure Pursuit are not magic