Start work on gyro abstraction

include/lemlib/devices/gyro/gyro.hpp

@@ -1 +1,62 @@
-#pragma once
+#pragma once
+
+#include <numeric>
+
+namespace lemlib {
+class Gyro {
+    public:
+        /**
+         * @brief Calibrate the gyro
+         *
+         * @param blocking whether the function should block until calibration is complete
+         * @return true calibration failed
+         * @return false calibration succeeded, or blocking is set to false
+         */
+        virtual bool calibrate(bool blocking = false);
+        /**
+         * @brief Get wether the gyro is calibrating or not
+         *
+         * @return true Gyro is calibrating
+         * @return false Gyro is not calibrating
+         */
+        virtual bool isCalibrating();
+        /**
+         * @brief Get wether the gyro is calibrated or not
+         *
+         * @return true the gyro is calibrated
+         * @return false the gyro is not calibrated
+         */
+        virtual bool isCalibrated();
+        /**
+         * @brief Get whether the gyro is connected or not
+         *
+         * @return true the gyro is connected
+         * @return false the gyro is not connected
+         */
+        virtual bool isConnected();
+        /**
+         * @brief Get the heading of the gyro
+         *
+         * @note 0 is in the positive x direction, and heading increases counterclockwise
+         *
+         * @return float heading, in radians, locked from 0-2pi
+         */
+        virtual float getHeading();
+        /**
+         * @brief Get the orientation of the gyro
+         *
+         * @note 0 is in the positive x direction, and heading increases counterclockwise
+         *
+         * @return float orientation, in radians
+         */
+        virtual float getOrientation();
+        /**
+         * @brief Set the orientation of the gyro
+         *
+         * @brief 0 is in the positive x direction, and heading increases counterclockwise
+         *
+         * @param orientation orientation, in radians
+         */
+        virtual void setOrientation(float orientation);
+};
+} // namespace lemlib

include/lemlib/odom/arc.hpp

@@ -1,9 +1,10 @@
 #pragma once
 
 #include <vector>
-#include "pros/imu.hpp"
+#include <memory>
 #include "lemlib/odom/odom.hpp"
 #include "lemlib/devices/trackingWheel.hpp"
+#include "lemlib/devices/gyro/gyro.hpp"
 
 namespace lemlib {
 class ArcOdom : public Odom {
@@ -16,7 +17,7 @@ class ArcOdom : public Odom {
          * @param imus vector containing imus to be used
          */
         ArcOdom(std::vector<TrackingWheel>& verticals, std::vector<TrackingWheel>& horizontals,
-                std::vector<pros::Imu>& imus);
+                std::vector<std::shared_ptr<Gyro>>& gyros);
 
         /**
          * @brief Calibrate tracking wheels and inertial sensors
@@ -31,6 +32,6 @@ class ArcOdom : public Odom {
     private:
         std::vector<TrackingWheel> verticals;
         std::vector<TrackingWheel> horizontals;
-        std::vector<pros::Imu> imus;
+        std::vector<std::shared_ptr<Gyro>> gyros;
 };
 } // namespace lemlib

src/lemlib/odom/arc.cpp

@@ -14,10 +14,10 @@
  * any tracking wheel + imu setup
  */
 lemlib::ArcOdom::ArcOdom(std::vector<TrackingWheel>& verticals, std::vector<TrackingWheel>& horizontals,
-                         std::vector<pros::Imu>& imus)
+                         std::vector<std::shared_ptr<Gyro>>& gyros)
     : verticals(verticals),
       horizontals(horizontals),
-      imus(imus) {}
+      gyros(gyros) {}
 
 /**
  * Calibrate the sensors
@@ -26,7 +26,7 @@ lemlib::ArcOdom::ArcOdom(std::vector<TrackingWheel>& verticals, std::vector<Trac
  * calibration. The encoders will output errors if they fail to calibrate.
  */
 void lemlib::ArcOdom::calibrate() {
-    // loop through vertical tracking wheels
+    // calibrate vertical tracking wheels
     for (auto it = verticals.begin(); it != verticals.end(); it++) {
         if (it->reset()) verticals.erase(it); // remove the tracking wheel if calibration failed
     }
@@ -35,25 +35,19 @@ void lemlib::ArcOdom::calibrate() {
         if (it->reset()) horizontals.erase(it); // remove the tracking wheel if calibration failed
     }
 
-    // loop through imus
-    for (auto it = imus.begin(); it != imus.end(); it++) it->reset();
-    // We will spend a maximum of 3000 seconds calibrating imus before we give up
-    // we wil continue to try and calibrate the IMU's until we run out of time
-    Timer timer(3000);
-    std::vector<pros::Imu> newImus;
-    while (!timer.isDone() && imus.size() > 0) { // loop until the timer is done or imus are calibrated
-        for (auto it = imus.begin(); it != imus.end(); it++) { // loop through all IMUs
-            if (!it->is_calibrating() && !(errno == PROS_ERR || errno == ENODEV || errno == ENXIO)) {
-                // erase imu from old vector and add to new one
-                imus.erase(it);
-                newImus.push_back(*it);
-            }
+    // calibrate gyros
+    for (auto it = gyros.begin(); it != gyros.end(); it++) (**it).calibrate();
+    Timer timer(3000); // try calibrating gyros for 3000 ms
+    while (!timer.isDone()) {
+        for (auto it = gyros.begin(); it != gyros.end(); it++) { // loop through all IMUs
+            if (!(**it).isCalibrating() && !(**it).isCalibrated()) (**it).calibrate();
         }
         pros::delay(10);
     }
-    // output errors if any imus did not calibrate
-    for (auto it = imus.begin(); it != imus.end(); it++) infoSink()->error("Error: IMU failed to calibrate");
-    imus = newImus;
+    // if a gyro failed to calibrate, output an error
+    for (auto it = gyros.begin(); it != gyros.end(); it++) {
+        if (!(**it).isCalibrated()) infoSink()->error("Error: IMU failed to calibrate");
+    }
 }
 
 void lemlib::ArcOdom::update() {}