From 25f8d21c3368778abaea7ac87cace7432adeda06 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 4 Dec 2024 13:44:11 -0800 Subject: [PATCH 1/2] Added xyt set --- include/EZ-Template/drive/drive.hpp | 30 ++++++++++++++++++++++++++--- src/EZ-Template/drive/tracking.cpp | 10 ++++++++-- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/include/EZ-Template/drive/drive.hpp b/include/EZ-Template/drive/drive.hpp index 1368f1bb..28490817 100644 --- a/include/EZ-Template/drive/drive.hpp +++ b/include/EZ-Template/drive/drive.hpp @@ -575,7 +575,7 @@ 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); @@ -583,13 +583,37 @@ class Drive { /** * 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 index 92a5dfcb..9469c344 100644 --- 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 Drive::decide_vert_sensor(ez::tracking_wheel *tracker, bool is_tracker_enabled, float ime, float ime_track) { +std::pair 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) { From 9c3f7df969d3659a70c20d22d66f4acd5ed66ac0 Mon Sep 17 00:00:00 2001 From: Jess Zarchi Date: Wed, 4 Dec 2024 13:54:30 -0800 Subject: [PATCH 2/2] Changed example project --- EZ-Template-Example-Project/src/main.cpp | 10 +++++----- src/main.cpp | 10 +++++----- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/EZ-Template-Example-Project/src/main.cpp b/EZ-Template-Example-Project/src/main.cpp index 9455dba3..1373451b 100644 --- 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/src/main.cpp b/src/main.cpp index feb28fdd..7194563b 100644 --- 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