Added getAngleDelta() function encoder

include/lemlib/devices/encoder/encoder.hpp

@@ -18,13 +18,26 @@ class Encoder {
          *
          * @return float angle rotated by the encoder, in radians
          */
-        virtual float getAngle() const = 0;
+        virtual float getAngle() = 0;
+
+        /**
+         * @brief Get the angle rotated by the encoder since the last time it was checked, in radians
+         *
+         * @return float angle rotated by the encoder, in radians
+         */
+        float getAngleDelta() {
+            float prevAngle = lastAngle; // save lastAngle, as it will get reset when calling getAngle() below
+            return (getAngle() - prevAngle);
+        }
+
         /**
          * @brief Reset the encoder
          *
          * @return true encoder calibration failed
          * @return false encoder calibration succeeded
          */
-        virtual bool reset() const = 0;
+        virtual bool reset() = 0;
+    protected:
+        float lastAngle = 0;
 };
 } // namespace lemlib

include/lemlib/devices/encoder/motor.hpp

@@ -22,14 +22,14 @@ class MotorEncoder : public Encoder {
          *
          * @return float angle rotated by the motor encoders, in radians
          */
-        float getAngle() const override;
+        float getAngle() override;
         /**
          * @brief Reset the motor encoder
          *
          * @return true calibration failed
          * @return false calibration succeeded
          */
-        bool reset() const override;
+        bool reset() override;
     private:
         std::shared_ptr<pros::MotorGroup> motors;
         const float rpm;

include/lemlib/devices/encoder/optical.hpp

@@ -21,14 +21,14 @@ class OpticalEncoder : public Encoder {
          *
          * @return float angle rotated by the optical encoder, in radians
          */
-        float getAngle() const override;
+        float getAngle() override;
         /**
          * @brief Reset the optical encoder
          *
          * @return true calibration failed
          * @return false calibration succeeded
          */
-        bool reset() const override;
+        bool reset() override;
     private:
         pros::adi::Encoder optical;
         const float ratio;

include/lemlib/devices/encoder/rotation.hpp

@@ -20,14 +20,14 @@ class RotationEncoder : public Encoder {
          *
          * @return float angle rotated by the rotation sensor, in radians
          */
-        float getAngle() const override;
+        float getAngle() override;
         /**
          * @brief Reset the rotation sensor
          *
          * @return true calibration failed
          * @return false calibration succeeded
          */
-        bool reset() const override;
+        bool reset() override;
     private:
         pros::Rotation rotation;
         const float ratio;

src/lemlib/devices/encoder/motor.cpp

@@ -1,4 +1,5 @@
 #include <cmath>
+#include <numeric>
 #include "lemlib/util.hpp"
 #include "lemlib/devices/encoder/motor.hpp"
 
@@ -23,7 +24,7 @@ lemlib::MotorEncoder::MotorEncoder(std::shared_ptr<pros::MotorGroup> motors, flo
  * output rpm by the input rpm. Then we just multiply the output by 2 pi
  * to get angle in radians.
  */
-float lemlib::MotorEncoder::getAngle() const {
+float lemlib::MotorEncoder::getAngle() {
     // get gearboxes and encoder position for each motor in the group
     std::vector<pros::MotorGears> gearsets = motors->get_gearing_all();
     std::vector<double> positions = motors->get_position_all();
@@ -39,11 +40,17 @@ float lemlib::MotorEncoder::getAngle() const {
         }
         angles.push_back(positions[i] * (rpm / in) * 2 * M_PI);
     }
-    // return average of elements in the angles vector
-    return avg(angles);
+    // calc average of the angles
+    float angle = avg(angles);
+    lastAngle = angle;
+    return angle;
 }
 
 /**
  * Reset the motor encoders.
  */
-bool lemlib::MotorEncoder::reset() const { return (motors->tare_position()) ? 0 : 1; }
+bool lemlib::MotorEncoder::reset() {
+    lastAngle = 0;
+    getAngleDelta();
+    return (motors->tare_position()) ? 0 : 1;
+}

src/lemlib/devices/encoder/optical.cpp

@@ -18,9 +18,13 @@ lemlib::OpticalEncoder::OpticalEncoder(char topPort, char bottomPort, bool rever
  * Pretty straightforward, raw value from the encoder gets converted to rotations
  * which gets converted to radians
  */
-float lemlib::OpticalEncoder::getAngle() const { return (float(optical.get_value()) * (2 * M_PI) / 360) / ratio; }
+float lemlib::OpticalEncoder::getAngle() {
+    float angle = (float(optical.get_value()) * (2 * M_PI) / 360) / ratio;
+    lastAngle = angle;
+    return angle;
+}
 
 /**
  * Reset/calibrate the optical encoder
  */
-bool lemlib::OpticalEncoder::reset() const { return (optical.reset()) ? 0 : 1; }
+bool lemlib::OpticalEncoder::reset() { return (optical.reset()) ? 0 : 1; }

src/lemlib/devices/encoder/rotation.cpp

@@ -22,11 +22,16 @@ lemlib::RotationEncoder::RotationEncoder(int port, bool reversed, float ratio)
  * Pretty straightforward, raw value from the rotation sensor gets converted to rotations
  * which gets converted to radians
  */
-float lemlib::RotationEncoder::getAngle() const {
-    return (float(rotation.get_position()) / 36000) * (2 * M_PI) / ratio;
+float lemlib::RotationEncoder::getAngle() {
+    float angle = (float(rotation.get_position()) / 36000) * (2 * M_PI) / ratio;
+    lastAngle = angle;
+    return angle;
 }
 
 /**
  * Reset/calibrate the optical encoder
  */
-bool lemlib::RotationEncoder::reset() const { return (rotation.reset_position()) ? 0 : 1; }
+bool lemlib::RotationEncoder::reset() {
+    lastAngle = 0;
+    return (rotation.reset_position()) ? 0 : 1;
+}

src/lemlib/odom/arc.cpp

@@ -46,7 +46,7 @@ void lemlib::ArcOdom::calibrate() {
     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
+        for (auto it = gyros.begin(); it != gyros.end(); it++) { // continuously calibrate in case of failure
             if (!(**it).isCalibrating() && !(**it).isCalibrated()) (**it).calibrate();
         }
         pros::delay(10);
@@ -55,10 +55,112 @@ void lemlib::ArcOdom::calibrate() {
     // 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("Warning: IMU on port {} failed to calibrate! Removing", (**it).getPort());
+            infoSink()->warn("IMU on port {} failed to calibrate! Removing", (**it).getPort());
             gyros.erase(it);
         }
     }
 }
 
-void lemlib::ArcOdom::update() {}
+/**
+ * Update Arc Odom
+ *
+ * Tracking through arcs works through estimating the robot's change in position between
+ * angles as an arc, rather than a straight line, improving accuracy.
+ *
+ * This alg can either use tracking wheels to calculate angle, or preferably an imu.
+ * Theoretically you can get better performance with tracking wheels, but this is very
+ * difficult to achieve.
+ *
+ * 5225A has published a fantastic paper on this odom algorithm:
+ * http://thepilons.ca/wp-content/uploads/2018/10/Tracking.pdf
+ */
+void lemlib::ArcOdom::update() {
+    /*
+    // get the current sensor values
+    float vertical1Raw = 0;
+    float vertical2Raw = 0;
+    float horizontal1Raw = 0;
+    float horizontal2Raw = 0;
+    float imuRaw = 0;
+    if (sensors.vertical1 != nullptr) vertical1Raw = sensors.vertical1->getDistance();
+    if (sensors.vertical2 != nullptr) vertical2Raw = sensors.vertical2->getDistance();
+    if (sensors.horizontal1 != nullptr) horizontal1Raw = sensors.horizontal1->getDistance();
+    if (sensors.horizontal2 != nullptr) horizontal2Raw = sensors.horizontal2->getDistance();
+    if (sensors.imu != nullptr) imuRaw = degToRad(sensors.imu->get_rotation());
+
+    // calculate the change in sensor values
+    float deltaVertical1 = vertical1Raw - prevVertical1;
+    float deltaVertical2 = vertical2Raw - prevVertical2;
+    float deltaHorizontal1 = horizontal1Raw - prevHorizontal1;
+    float deltaHorizontal2 = horizontal2Raw - prevHorizontal2;
+    float deltaImu = imuRaw - prevImu;
+
+    // update the previous sensor values
+    prevVertical1 = vertical1Raw;
+    prevVertical2 = vertical2Raw;
+    prevHorizontal1 = horizontal1Raw;
+    prevHorizontal2 = horizontal2Raw;
+    prevImu = imuRaw;
+
+    // calculate the heading of the robot
+    // Priority:
+    // 1. IMU
+    // 2. Horizontal tracking wheels
+    // 3. Vertical tracking wheels
+    float heading = pose.theta;
+    // calculate heading with inertial sensor if it exists
+    if (sensors.imu != nullptr) heading += deltaImu;
+    // else, use horizontal tracking wheels if they both exist
+    else if (sensors.horizontal1 != nullptr && sensors.horizontal2 != nullptr)
+        heading += (deltaHorizontal1 - deltaHorizontal2) /
+                   (sensors.horizontal1->getOffset() - sensors.horizontal2->getOffset());
+    else
+        heading +=
+            (deltaVertical1 - deltaVertical2) / (sensors.vertical1->getOffset() - sensors.vertical2->getOffset());
+    float deltaHeading = heading - pose.theta;
+    float avgHeading = pose.theta + deltaHeading / 2;
+
+    // choose tracking wheels to use
+    TrackingWheel* verticalWheel = sensors.vertical1;
+    TrackingWheel* horizontalWheel = nullptr;
+    // horizontal tracking wheels
+    if (sensors.horizontal1 != nullptr) horizontalWheel = sensors.horizontal1;
+    if (sensors.horizontal2 != nullptr) horizontalWheel = sensors.horizontal2;
+    // get raw values
+    float rawVertical = 0;
+    float rawHorizontal = 0;
+    rawVertical = verticalWheel->getDistance();
+    if (horizontalWheel != nullptr) rawHorizontal = horizontalWheel->getDistance();
+    // get offsets
+    float horizontalOffset = 0;
+    float verticalOffset = 0;
+    verticalOffset = verticalWheel->getOffset();
+    if (horizontalWheel != nullptr) horizontalOffset = horizontalWheel->getOffset();
+
+    // calculate change in x and y
+    float deltaX = 0;
+    float deltaY = 0;
+    deltaY = rawVertical - prevVertical;
+    if (horizontalWheel != nullptr) deltaX = rawHorizontal - prevHorizontal;
+    prevVertical = rawVertical;
+    prevHorizontal = rawHorizontal;
+
+    // calculate local x and y
+    float localX = 0;
+    float localY = 0;
+    if (deltaHeading == 0) { // prevent divide by 0
+        localX = deltaX;
+        localY = deltaY;
+    } else {
+        localX = 2 * sin(deltaHeading / 2) * (deltaX / deltaHeading + horizontalOffset);
+        localY = 2 * sin(deltaHeading / 2) * (deltaY / deltaHeading + verticalOffset);
+    }
+
+    // calculate global x and y
+    pose.x += localY * sin(avgHeading);
+    pose.y += localY * cos(avgHeading);
+    pose.x += localX * -cos(avgHeading);
+    pose.y += localX * sin(avgHeading);
+    pose.theta = heading;
+    */
+}