add getPort

Author: Ynthra

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
          *

src/hardware/IMU/V5InertialSensor.cpp

@@ -1,6 +1,7 @@
 #include "hardware/IMU/V5InertialSensor.hpp"
 #include "hardware/Port.hpp"
 #include "pros/imu.hpp"
+#include <cstdint>
 #include <mutex>
 
 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;