diff --git a/include/hardware/IMU/V5InertialSensor.hpp b/include/hardware/IMU/V5InertialSensor.hpp index f202ad3..67c12a9 100644 --- a/include/hardware/IMU/V5InertialSensor.hpp +++ b/include/hardware/IMU/V5InertialSensor.hpp @@ -46,6 +46,22 @@ class V5InertialSensor : public IMU { * @endcode */ static V5InertialSensor from_pros_imu(pros::Imu imu, Number scalar = 1.0); + /** + * @brief Get the port the inertial sensor is connected to + * + * This function returns the port number of the inertial sensor. + * + * @return SmartPort the port the inertial sensor is connected to + * + * @b Example: + * @code {.cpp} + * void initialize() { + * lemlib::V5InertialSensor imu(1); + * std::cout << "Inertial sensor is connected to port " << imu.getPort() << std::endl; + * } + * @endcode + */ + SmartPort getPort() const; /** * @brief calibrate the V5 Inertial Sensor * diff --git a/src/hardware/IMU/V5InertialSensor.cpp b/src/hardware/IMU/V5InertialSensor.cpp index 992a4b2..41e410a 100644 --- a/src/hardware/IMU/V5InertialSensor.cpp +++ b/src/hardware/IMU/V5InertialSensor.cpp @@ -1,6 +1,7 @@ #include "hardware/IMU/V5InertialSensor.hpp" #include "hardware/Port.hpp" #include "pros/imu.hpp" +#include #include namespace lemlib { @@ -17,6 +18,12 @@ V5InertialSensor V5InertialSensor::from_pros_imu(pros::IMU imu, Number scalar) { return V5InertialSensor({imu.get_port(), runtime_check_port}, scalar); } +SmartPort V5InertialSensor::getPort() const { + std::lock_guard lock(m_mutex); + SmartPort port(m_imu.get_port(), DynamicPort {}); + return port; +} + int32_t V5InertialSensor::calibrate() { std::lock_guard lock(m_mutex); m_offset = 0_stRot;