From 7a66f2f6852d6e3381b522d9bab16a485515ea25 Mon Sep 17 00:00:00 2001 From: Gracelu128 <112266075+Gracelu128@users.noreply.github.com> Date: Sun, 5 May 2024 18:25:16 -0400 Subject: [PATCH] =?UTF-8?q?=F0=9F=90=9BFixed=20Imu::is=5Fcalibrating=20fun?= =?UTF-8?q?ction=20for=20PROS=204=20#626=20(#629)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Fixed Imu::is_calibrating function Updated imu status enums to properly reflect values returned by get_status. Also modifed imu::is_calibrating so it returns the correct value. * Bugfixes from the pros 3 version --------- Co-authored-by: noam987 Co-authored-by: noam987 <50681033+noam987@users.noreply.github.com> --- include/pros/imu.h | 29 ++++++++++++- include/pros/imu.hpp | 21 +++++++++- src/devices/vdml_imu.c | 89 ++++++++++++++++++++++------------------ src/devices/vdml_imu.cpp | 10 ++++- 4 files changed, 106 insertions(+), 43 deletions(-) diff --git a/include/pros/imu.h b/include/pros/imu.h index bf724ab05..63cc73848 100644 --- a/include/pros/imu.h +++ b/include/pros/imu.h @@ -41,13 +41,25 @@ namespace pros { * @brief Indicates IMU status. */ typedef enum imu_status_e { + E_IMU_STATUS_READY = 0, // IMU is connected but not currently calibrating /** The IMU is calibrating */ - E_IMU_STATUS_CALIBRATING = 0x01, + E_IMU_STATUS_CALIBRATING = 1, /** Used to indicate that an error state was reached in the imu_get_status function,\ not that the IMU is necessarily in an error state */ E_IMU_STATUS_ERROR = 0xFF, } imu_status_e_t; +typedef enum imu_orientation_e { + E_IMU_Z_UP = 0, // IMU has the Z axis UP (VEX Logo facing DOWN) + E_IMU_Z_DOWN = 1, // IMU has the Z axis DOWN (VEX Logo facing UP) + E_IMU_X_UP = 2, // IMU has the X axis UP + E_IMU_X_DOWN = 3, // IMU has the X axis DOWN + E_IMU_Y_UP = 4, // IMU has the Y axis UP + E_IMU_Y_DOWN = 5, // IMU has the Y axis DOWN + E_IMU_ORIENTATION_ERROR = 0xFF // NOTE: used for returning an error from the get_physical_orientation function, not + // that the IMU is necessarily in an error state +} imu_orientation_e_t; + /** * \struct quaternion_s_t */ @@ -934,6 +946,21 @@ int32_t imu_set_roll(uint8_t port, double target); */ int32_t imu_set_yaw(uint8_t port, double target); +/** + * Returns the physical orientation of the IMU + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \returns The orientation of the Inertial Sensor or PROS_ERR if an error occured. + * + */ +imu_orientation_e_t imu_get_physical_orientation(uint8_t port); + /** @} */ /** @} */ diff --git a/include/pros/imu.hpp b/include/pros/imu.hpp index a13178899..0cee565ce 100644 --- a/include/pros/imu.hpp +++ b/include/pros/imu.hpp @@ -40,8 +40,9 @@ namespace pros { */ enum class ImuStatus { + ready = 0, /** The IMU is calibrating */ - calibrating = 0x01, + calibrating = 19, /** Used to indicate that an error state was reached in the imu_get_status function,\ not that the IMU is necessarily in an error state */ error = 0xFF, @@ -81,7 +82,9 @@ class Imu : public Device { * } * \endcode */ - Imu(const std::uint8_t port) : Device(port, DeviceType::imu){}; + + Imu(const std::uint8_t port) : Device(port, DeviceType::imu) {}; + Imu(const Device& device) : Imu(device.get_port()){}; @@ -1008,6 +1011,20 @@ class Imu : public Device { * \endcode */ virtual bool is_calibrating() const; + /** + * Returns the physical orientation of the IMU + * + * This function uses the following values of errno when an error state is + * reached: + * ENXIO - The given value is not within the range of V5 ports (1-21). + * ENODEV - The port cannot be configured as an Inertial Sensor + * + * \param port + * The V5 Inertial Sensor port number from 1-21 + * \returns The physical orientation of the Inertial Sensor or PROS_ERR if an error occured. + * + */ + virtual imu_orientation_e_t get_physical_orientation() const; /** * This is the overload for the << operator for printing to streams diff --git a/src/devices/vdml_imu.c b/src/devices/vdml_imu.c index b4cf27a9f..f2cd73525 100644 --- a/src/devices/vdml_imu.c +++ b/src/devices/vdml_imu.c @@ -11,6 +11,7 @@ */ #include + #include "pros/imu.h" #include "v5_api.h" #include "vdml/registry.h" @@ -28,9 +29,9 @@ } #define IMU_RESET_FLAG_SET_TIMEOUT 1000 -#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin +#define IMU_RESET_TIMEOUT 3000 // Canonically this should be 2s, but 3s for good margin -typedef struct __attribute__ ((packed)) imu_reset_data { +typedef struct __attribute__((packed)) imu_reset_data { double heading_offset; double rotation_offset; double pitch_offset; @@ -56,8 +57,8 @@ int32_t imu_reset(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); + device = device; // suppressing compiler warning + } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); port_mutex_give(port - 1); return 1; } @@ -80,8 +81,8 @@ int32_t imu_reset_blocking(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); + device = device; // suppressing compiler warning + } while (!(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING)); // same concept here, we add a blocking delay for the blocking version to wait // until the IMU calibrating flag is cleared do { @@ -94,8 +95,8 @@ int32_t imu_reset_blocking(uint8_t port) { errno = EAGAIN; return PROS_ERR; } - device = device; // suppressing compiler warning - } while(vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); + device = device; // suppressing compiler warning + } while (vexDeviceImuStatusGet(device->device_info) & E_IMU_STATUS_CALIBRATING); port_mutex_give(port - 1); return 1; } @@ -118,16 +119,18 @@ int32_t imu_set_data_rate(uint8_t port, uint32_t rate) { double imu_get_rotation(uint8_t port) { claim_port_f(port - 1, E_DEVICE_IMU); ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); - double rtn = vexDeviceImuHeadingGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset; + double rtn = vexDeviceImuHeadingGet(device->device_info) + + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->rotation_offset; return_port(port - 1, rtn); } double imu_get_heading(uint8_t port) { claim_port_f(port - 1, E_DEVICE_IMU); ERROR_IMU_STILL_CALIBRATING(port, device, PROS_ERR_F); - double rtn = vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset; + double rtn = + vexDeviceImuDegreesGet(device->device_info) + ((imu_data_s_t*)registry_get_device(port - 1)->pad)->heading_offset; // Restricting value to raw boundaries - return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double) IMU_HEADING_MAX)); + return_port(port - 1, fmod((rtn + IMU_HEADING_MAX), (double)IMU_HEADING_MAX)); } #define QUATERNION_ERR_INIT \ @@ -154,7 +157,7 @@ quaternion_s_t imu_get_quaternion(uint8_t port) { double cp = cos(DEGTORAD * pitch * 0.5); double sp = sin(DEGTORAD * pitch * 0.5); double cr = cos(DEGTORAD * roll * 0.5); - double sr = sin(DEGTORAD * roll * 0.5); + double sr = sin(DEGTORAD * roll * 0.5); rtn.w = cr * cp * cy + sr * sp * sy; rtn.x = sr * cp * cy - cr * sp * sy; @@ -270,9 +273,9 @@ imu_status_e_t imu_get_status(uint8_t port) { return_port(port - 1, rtn); } -//Reset Functions: -int32_t imu_tare(uint8_t port){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +// Reset Functions: +int32_t imu_tare(uint8_t port) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -287,33 +290,33 @@ int32_t imu_tare(uint8_t port){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_tare_euler(uint8_t port){ - return imu_set_euler(port, (euler_s_t){0,0,0}); +int32_t imu_tare_euler(uint8_t port) { + return imu_set_euler(port, (euler_s_t){0, 0, 0}); } -int32_t imu_tare_heading(uint8_t port){ - return imu_set_heading(port, 0); +int32_t imu_tare_heading(uint8_t port) { + return imu_set_heading(port, 0); } -int32_t imu_tare_rotation(uint8_t port){ - return imu_set_rotation(port, 0); +int32_t imu_tare_rotation(uint8_t port) { + return imu_set_rotation(port, 0); } -int32_t imu_tare_pitch(uint8_t port){ - return imu_set_pitch(port, 0); +int32_t imu_tare_pitch(uint8_t port) { + return imu_set_pitch(port, 0); } -int32_t imu_tare_roll(uint8_t port){ - return imu_set_roll(port, 0); +int32_t imu_tare_roll(uint8_t port) { + return imu_set_roll(port, 0); } -int32_t imu_tare_yaw(uint8_t port){ - return imu_set_yaw(port, 0); +int32_t imu_tare_yaw(uint8_t port) { + return imu_set_yaw(port, 0); } -//Setter Functions: -int32_t imu_set_rotation(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +// Setter Functions: +int32_t imu_set_rotation(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -325,8 +328,8 @@ int32_t imu_set_rotation(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_heading(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_heading(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -340,8 +343,8 @@ int32_t imu_set_heading(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_pitch(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_pitch(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -355,8 +358,8 @@ int32_t imu_set_pitch(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_roll(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_roll(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -370,8 +373,8 @@ int32_t imu_set_roll(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_yaw(uint8_t port, double target){ - if (!claim_port_try(port - 1, E_DEVICE_IMU)) { +int32_t imu_set_yaw(uint8_t port, double target) { + if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } v5_smart_device_s_t* device = registry_get_device(port - 1); @@ -385,7 +388,7 @@ int32_t imu_set_yaw(uint8_t port, double target){ return_port(port - 1, PROS_SUCCESS); } -int32_t imu_set_euler(uint8_t port, euler_s_t target){ +int32_t imu_set_euler(uint8_t port, euler_s_t target) { if (!claim_port_try(port - 1, E_DEVICE_IMU)) { return PROS_ERR; } @@ -404,3 +407,11 @@ int32_t imu_set_euler(uint8_t port, euler_s_t target){ data->yaw_offset = target.yaw - euler_values.yaw; return_port(port - 1, PROS_SUCCESS); } + +imu_orientation_e_t imu_get_physical_orientation(uint8_t port) { + imu_status_e_t status = imu_get_status(port); + if (status == E_IMU_STATUS_ERROR) { + return E_IMU_ORIENTATION_ERROR; + } + return (status >> 1) & 7; +} diff --git a/src/devices/vdml_imu.cpp b/src/devices/vdml_imu.cpp index eb30ec806..8c5b23750 100644 --- a/src/devices/vdml_imu.cpp +++ b/src/devices/vdml_imu.cpp @@ -76,7 +76,11 @@ pros::ImuStatus Imu::get_status() const { } bool Imu::is_calibrating() const { - return (int)get_status() & (int)(pros::ImuStatus::calibrating); + imu_status_e_t status = pros::c::imu_get_status(_port); + if (status == E_IMU_STATUS_ERROR) { + return false; + } + return status & E_IMU_STATUS_CALIBRATING; } std::int32_t Imu::tare_heading() const { @@ -131,6 +135,10 @@ std::int32_t Imu::tare() const { return pros::c::imu_tare(_port); } +imu_orientation_e_t Imu::get_physical_orientation() const { + return pros::c::imu_get_physical_orientation(_port); +} + Imu Imu::get_imu() { static int curr_imu_port = 0; curr_imu_port = curr_imu_port % 21;