Merge branch 'refactor' of https://github.com/LemLib/LemLib into refa…

…ctor

include/lemlib/pose.hpp

@@ -35,7 +35,7 @@ class Pose {
          * @param other other pose
          * @return Pose
          */
-        Pose operator+(const Pose& other);
+        Pose operator+(const Pose& other) const;
         /**
          * @brief Set the value of this pose to its value + the value of another pose
          *
@@ -48,7 +48,7 @@ class Pose {
          * @param other other pose
          * @return Pose
          */
-        Pose operator-(const Pose& other);
+        Pose operator-(const Pose& other) const;
         /**
          * @brief Set the value of this pose to its value - the value of another pose
          *
@@ -61,57 +61,57 @@ class Pose {
          * @param other other pose
          * @return Pose
          */
-        float operator*(const Pose& other);
+        float operator*(const Pose& other) const;
         /**
          * @brief Multiply a pose by a float
          *
          * @param other float
          * @return Pose
          */
-        Pose operator*(const float& other);
+        Pose operator*(const float& other) const;
         /**
          * @brief Divide a pose by a float
          *
          * @param other float
          * @return Pose
          */
-        Pose operator/(const float& other);
+        Pose operator/(const float& other) const;
         /**
          * @brief Check if two poses are equal
          *
          * @param other the other pose
          * @return bool
          */
-        bool operator==(const Pose& other);
+        bool operator==(const Pose& other) const;
         /**
          * @brief Check if two poses are not equal
          *
          * @param other the other pose
          * @return bool
          */
-        bool operator!=(const Pose& other);
+        bool operator!=(const Pose& other) const;
         /**
          * @brief Linearly interpolate between two poses
          *
          * @param other the other pose
          * @param t t value
          * @return Pose
          */
-        Pose lerp(Pose other, float t);
+        Pose lerp(const Pose other, float t) const;
         /**
          * @brief Get the distance between two poses
          *
          * @param other the other pose
          * @return float
          */
-        float distance(Pose other);
+        float distance(const Pose other) const;
         /**
          * @brief Get the angle between two poses
          *
          * @param other the other pose
          * @return float in radians
          */
-        float angle(Pose other);
+        float angle(const Pose other) const;
         /**
          * @brief Rotate a pose by an angle
          *
@@ -120,7 +120,7 @@ class Pose {
          * @param angle angle in radians
          * @return Pose
          */
-        Pose rotate(float angle);
+        Pose rotate(float angle) const;
 };
 
 /**

src/lemlib/odom/differentialArc.cpp

@@ -29,30 +29,31 @@ DifferentialArc::DifferentialArc(std::vector<TrackingWheel>& verticals, std::vec
  * calibration. The encoders will output errors if they fail to calibrate.
  */
 void DifferentialArc::calibrate(bool calibrateGyros) {
+    std::vector<TrackingWheel> newVerticals = {};
+    std::vector<TrackingWheel> newHorizontals = {};
+    std::vector<TrackingWheel> newDrivetrain = {};
+    std::vector<std::shared_ptr<Gyro>> newGyros = {};
+
     // calibrate vertical tracking wheels
     for (auto it = verticals.begin(); it != verticals.end(); it++) {
         if (it->reset()) {
             infoSink()->warn("Vertical tracker at offset {} failed calibration!", it->getOffset());
-            verticals.erase(it);
-        }
+        } else newVerticals.push_back(*it);
     }
 
     // calibrate horizontal tracking wheels
     for (auto it = horizontals.begin(); it != horizontals.end(); it++) {
         if (it->reset()) {
             infoSink()->warn("Horizontal tracker at offset {} failed calibration!", it->getOffset());
-            horizontals.erase(it);
-        }
+        } else newHorizontals.push_back(*it);
     }
 
     // calibrate drivetrain motors
     for (auto it = drivetrain.begin(); it != drivetrain.end(); it++) {
         if (it->reset()) {
-            if (sgn(it->getOffset() == 1))
-                infoSink()->warn("Left drivetrain motor failed to calibrate!", it->getOffset());
-            else infoSink()->warn("Right drivetrain motor failed to calibrate!", it->getOffset());
-            drivetrain.erase(it);
-        }
+            if (sgn(it->getOffset() == 1)) infoSink()->warn("Left drivetrain motor failed to calibrate!");
+            else infoSink()->warn("Right drivetrain motor failed to calibrate!");
+        } else newDrivetrain.push_back(*it);
     }
 
     if (!calibrateGyros) return; // return if we don't need to calibrate gyros
@@ -66,13 +67,18 @@ void DifferentialArc::calibrate(bool calibrateGyros) {
         pros::delay(10);
     }
 
-    // if a gyro failed to calibrate, output an error and erase the gyro
     for (auto it = gyros.begin(); it != gyros.end(); it++) {
         if (!(**it).isCalibrated()) {
-            infoSink()->warn("IMU on port {} failed to calibrate! Removing", (**it).getPort());
-            gyros.erase(it);
+            infoSink()->warn("IMU on port {} failed to calibrate! Removing...", (**it).getPort());
+        } else {
+            newGyros.push_back(*it);
         }
     }
+
+    verticals = newVerticals;
+    horizontals = newHorizontals;
+    drivetrain = newDrivetrain;
+    gyros = newGyros;
 }
 
 /**

src/lemlib/pose.cpp

@@ -35,7 +35,7 @@ Pose::Pose(float x, float y, float theta)
  * @param other other pose
  * @return Pose
  */
-Pose Pose::operator+(const Pose& other) { return Pose(x + other.x, y + other.y, theta); }
+Pose Pose::operator+(const Pose& other) const { return Pose(x + other.x, y + other.y, theta); }
 
 /**
  * @brief Set the value of this pose to its value + the value of another pose
@@ -54,7 +54,7 @@ void Pose::operator+=(const Pose& other) {
  * @param other other pose
  * @return Pose
  */
-Pose Pose::operator-(const Pose& other) { return Pose(x - other.x, y - other.y, theta); }
+Pose Pose::operator-(const Pose& other) const { return Pose(x - other.x, y - other.y, theta); }
 
 /**
  * @brief Set the value of this pose to its value - the value of another pose
@@ -73,39 +73,39 @@ void Pose::operator-=(const Pose& other) {
  * @param other other pose
  * @return Pose
  */
-float Pose::operator*(const Pose& other) { return x * other.x + y * other.y; }
+float Pose::operator*(const Pose& other) const { return x * other.x + y * other.y; }
 
 /**
  * @brief Multiply a pose by a float
  *
  * @param other float
  * @return Pose
  */
-Pose Pose::operator*(const float& other) { return Pose(x * other, y * other, theta); }
+Pose Pose::operator*(const float& other) const { return Pose(x * other, y * other, theta); }
 
 /**
  * @brief Divide a pose by a float
  *
  * @param other float
  * @return Pose
  */
-Pose Pose::operator/(const float& other) { return Pose(x / other, y / other, theta); }
+Pose Pose::operator/(const float& other) const { return Pose(x / other, y / other, theta); }
 
 /**
  * @brief Check if two poses are equal
  *
  * @param other the other pose
  * @return bool
  */
-bool Pose::operator==(const Pose& other) { return x == other.x && y == other.y && theta == other.theta; }
+bool Pose::operator==(const Pose& other) const { return x == other.x && y == other.y && theta == other.theta; }
 
 /**
  * @brief Check if two poses are not equal
  *
  * @param other the other pose
  * @return bool
  */
-bool Pose::operator!=(const Pose& other) { return x != other.x || y != other.y || theta != other.theta; }
+bool Pose::operator!=(const Pose& other) const { return x != other.x || y != other.y || theta != other.theta; }
 
 /**
  * @brief Linearly interpolate between two poses
@@ -114,31 +114,31 @@ bool Pose::operator!=(const Pose& other) { return x != other.x || y != other.y |
  * @param t t value
  * @return Pose
  */
-Pose Pose::lerp(Pose other, float t) { return Pose(x + (other.x - x) * t, y + (other.y - y) * t, theta); }
+Pose Pose::lerp(Pose other, float t) const { return Pose(x + (other.x - x) * t, y + (other.y - y) * t, theta); }
 
 /**
  * @brief Get the distance between two poses
  *
  * @param other the other pose
  * @return float
  */
-float Pose::distance(Pose other) { return std::hypot(x - other.x, y - other.y); }
+float Pose::distance(Pose other) const { return std::hypot(x - other.x, y - other.y); }
 
 /**
  * @brief Get the angle between two poses
  *
  * @param other the other pose
  * @return float in radians
  */
-float Pose::angle(Pose other) { return std::atan2(other.y - y, other.x - x); }
+float Pose::angle(Pose other) const { return std::atan2(other.y - y, other.x - x); }
 
 /**
  * @brief Rotate a pose by an angle
  *
  * @param angle angle in radians
  * @return Pose
  */
-Pose Pose::rotate(float angle) {
+Pose Pose::rotate(float angle) const {
     const float cosAngle = std::cos(angle);
     const float sinAngle = std::sin(angle);