Merge branch 'refactor' into refactor

include/lemlib/pid.hpp

@@ -10,9 +10,44 @@
  */
 #pragma once
 #include <string>
+#include <set>
 #include "pros/rtos.hpp"
 
 namespace lemlib {
+struct Gains {
+        float kF = 0;
+        float kA = 0;
+        float kP = 0;
+        float kI = 0;
+        float kD = 0;
+};
+
+/** @brief A function taking the target and the (target, Gains) elements adjacent to it, computing the resulting gains
+ */
+using Interpolator = std::function<Gains(float, std::pair<float, Gains>, std::pair<float, Gains>)>;
+
+/**
+ * @brief A gain interpolator that selects the gains with the closest target
+ *
+ * @param target the target at which to interpolate
+ * @param below the lower adjacent (target, Gains) value
+ * @param above the higher adjacent (target, Gains) value
+ *
+ * @returns the interpolated gains
+ */
+Gains interpolateNearest(float target, std::pair<float, Gains> below, std::pair<float, Gains> above);
+
+/**
+ * @brief A gain interpolator that linearly interpolates gains
+ *
+ * @param target the target at which to interpolate
+ * @param below the lower adjacent (target, Gains) value
+ * @param above the higher adjacent (target, Gains) value
+ *
+ * @returns the interpolated gains
+ */
+Gains interpolateLinear(float, std::pair<float, Gains>, std::pair<float, Gains>);
+
 /**
  * @brief Feedforward, Acceleration, Proportional, Integral, Derivative PID controller
  *
@@ -33,6 +68,34 @@ class FAPID {
          * @param name name of the FAPID. Used for logging
          */
         FAPID(float kF, float kA, float kP, float kI, float kD, std::string name);
+
+        /**
+         * @brief Construct a new FAPID
+         *
+         * @param gains the gains for the FAPID to use
+         * @param name name of the FAPID. Used for logging
+         */
+        FAPID(Gains gains, std::string name);
+
+        /**
+         * @brief Construct a new FAPID
+         *
+         * @param gains the default gains for the FAPID to use
+         * @param scheduled a set of (target, Gains) pairs to use for gain scheduling
+         * @param name name of the FAPID. Used for logging
+         */
+        FAPID(Gains gains, std::set<std::pair<float, Gains>> scheduled, std::string name);
+
+        /**
+         * @brief Construct a new FAPID
+         *
+         * @param gains the default gains for the FAPID to use
+         * @param scheduled a set of (target, Gains) pairs to use for gain scheduling
+         * @param interpolator the function to use when interpolating gains when scheduling
+         * @param name name of the FAPID. Used for logging
+         */
+        FAPID(Gains gains, std::set<std::pair<float, Gains>> scheduled, Interpolator interpolator, std::string name);
+
         /**
          * @brief Set gains
          *
@@ -43,6 +106,28 @@ class FAPID {
          * @param kD derivative gain, multiplied by change in error and added to output
          */
         void setGains(float kF, float kA, float kP, float kI, float kD);
+
+        /**
+         * @brief Set gains
+         *
+         * @param gains the new gains
+         */
+        void setGains(Gains gains);
+
+        /**
+         * @brief Set scheduled gains
+         *
+         * @param gains the new scheduled gains
+         */
+        void setScheduledGains(std::set<std::pair<float, Gains>> scheduled);
+
+        /**
+         * @brief Set gain interpolator
+         *
+         * @param gains the new gain interpolator
+         */
+        void setGainInterpolator(Interpolator interpolator);
+
         /**
          * @brief Set the exit conditions
          *
@@ -63,6 +148,7 @@ class FAPID {
          * @return float - output
          */
         float update(float target, float position, bool log = false);
+
         /**
          * @brief Reset the FAPID
          */
@@ -96,12 +182,17 @@ class FAPID {
          */
         static void init();
     private:
-        float kF;
-        float kA;
-        float kP;
-        float kI;
-        float kD;
+        // An ordered set of (target, Gains) pairs for gain scheduling
+        std::set<std::pair<float, Gains>> scheduledGains = {};
 
+        // The gains the PID will use
+        Gains currentGains;
+
+        // A function taking the target and the elements adjacent to the target in the (target, gain) set, computing the
+        // final gains
+        Interpolator gainInterpolator = interpolateNearest;
+
+        float previousTarget = 0;
         float largeError;
         float smallError;
         int largeTime = 0;
@@ -122,4 +213,5 @@ class FAPID {
         static pros::Task* logTask;
         static pros::Mutex logMutex;
 };
+
 } // namespace lemlib

include/lemlib/util.hpp

@@ -124,4 +124,17 @@ Waypoint closestWaypoint(const std::vector<Waypoint>& waypoints, const Pose& tar
  * multiple intersections, the first one will be returned
  */
 Pose circleLineIntersect(Pose p1, Pose p2, Pose center, float radius);
+
+/*
+ * @brief Linearly interpolate two points
+ *
+ * @param x the x value to interpolate
+ * @param x1 the x value of the lower adjacent coordinate
+ * @param y1 the y value of the lower adjacent coordinate
+ * @param x2 the x value of the upper adjacent coordinate
+ * @param y2 the y value of the upper adjacent coordinate
+ * @return the interpolated y value
+ */
+float linearInterp(float x, float x1, float y1, float x2, float y2);
+
 } // namespace lemlib

src/lemlib/pid.cpp

@@ -9,7 +9,10 @@
  *
  */
 
+#include <algorithm>
+#include <cmath>
 #include <iostream>
+#include <limits>
 #include <math.h>
 #include "lemlib/pid.hpp"
 #include "lemlib/util.hpp"
@@ -30,12 +33,48 @@ pros::Mutex FAPID::logMutex = pros::Mutex();
  * @param kD derivative gain, multiplied by change in error and added to output
  * @param name name of the FAPID. Used for logging
  */
-FAPID::FAPID(float kF, float kA, float kP, float kI, float kD, std::string name) {
-    this->kF = kF;
-    this->kA = kA;
-    this->kP = kP;
-    this->kI = kI;
-    this->kD = kD;
+lemlib::FAPID::FAPID(float kF, float kA, float kP, float kI, float kD, std::string name) {
+    currentGains = Gains {kF, kA, kP, kI, kD};
+    this->name = name;
+}
+
+/**
+ * @brief Construct a new FAPID
+ *
+ * @param gains the gains for the FAPID to use
+ * @param name name of the FAPID. Used for logging
+ */
+lemlib::FAPID::FAPID(Gains gains, std::string name) {
+    currentGains = gains;
+    this->name = name;
+}
+
+/**
+ * @brief Construct a new FAPID
+ *
+ * @param gains the default gains for the FAPID to use
+ * @param scheduled a set of (target, Gains) pairs to use for gain scheduling
+ * @param name name of the FAPID. Used for logging
+ */
+lemlib::FAPID::FAPID(Gains gains, std::set<std::pair<float, Gains>> scheduled, std::string name) {
+    currentGains = gains;
+    scheduledGains = scheduled;
+    this->name = name;
+}
+
+/**
+ * @brief Construct a new FAPID
+ *
+ * @param gains the default gains for the FAPID to use
+ * @param scheduled a set of (target, Gains) pairs to use for gain scheduling
+ * @param interpolator the function to use when interpolating gains when scheduling
+ * @param name name of the FAPID. Used for logging
+ */
+lemlib::FAPID::FAPID(Gains gains, std::set<std::pair<float, Gains>> scheduled, Interpolator interpolator,
+                     std::string name) {
+    currentGains = gains;
+    scheduledGains = scheduled;
+    gainInterpolator = interpolator;
     this->name = name;
 }
 
@@ -48,14 +87,32 @@ FAPID::FAPID(float kF, float kA, float kP, float kI, float kD, std::string name)
  * @param kI integral gain, multiplied by total error and added to output
  * @param kD derivative gain, multiplied by change in error and added to output
  */
-void FAPID::setGains(float kF, float kA, float kP, float kI, float kD) {
-    this->kF = kF;
-    this->kA = kA;
-    this->kP = kP;
-    this->kI = kI;
-    this->kD = kD;
+void lemlib::FAPID::setGains(float kF, float kA, float kP, float kI, float kD) {
+    auto gains = Gains {kF, kA, kP, kI, kD};
+    currentGains = gains;
 }
 
+/**
+ * @brief Set gains
+ *
+ * @param gains the new gains
+ */
+void lemlib::FAPID::setGains(Gains gains) { currentGains = gains; }
+
+/**
+ * @brief Set scheduled gains
+ *
+ * @param gains the new scheduled gains
+ */
+void lemlib::FAPID::setScheduledGains(std::set<std::pair<float, Gains>> scheduled) { scheduledGains = scheduled; }
+
+/**
+ * @brief Set gain interpolator
+ *
+ * @param gains the new gain interpolator
+ */
+void lemlib::FAPID::setGainInterpolator(Interpolator interpolator) { gainInterpolator = interpolator; }
+
 /**
  * @brief Set the exit conditions
  *
@@ -73,6 +130,50 @@ void FAPID::setExit(float largeError, float smallError, int largeTime, int small
     this->maxTime = maxTime;
 }
 
+/**
+ * @brief A gain interpolator that selects the gains with the closest target
+ *
+ * @param target the target at which to interpolate
+ * @param below the lower adjacent (target, Gains) value
+ * @param above the higher adjacent (target, Gains) value
+ *
+ * @returns the interpolated gains
+ */
+lemlib::Gains interpolateNearest(float target, std::pair<float, Gains> below, std::pair<float, Gains> above) {
+    if (std::abs(target - below.first) < std::abs(target - above.first)) {
+        return below.second;
+    } else {
+        return above.second;
+    }
+}
+
+/**
+ * @brief A gain interpolator that linearly interpolates gains
+ *
+ * @param target the target at which to interpolate
+ * @param below the lower adjacent (target, Gains) value
+ * @param above the higher adjacent (target, Gains) value
+ *
+ * @returns the interpolated gains
+ */
+lemlib::Gains interpolateLinear(float target, std::pair<float, Gains> below, std::pair<float, Gains> above) {
+    auto nearest = lemlib::interpolateNearest(target, below, above);
+    auto [x1, y1] = below;
+    auto [x2, y2] = above;
+
+    // Should kF and kA be interpolated?
+    // It seems that feedforward constants should remain the same
+
+    auto kF = nearest.kF;
+    auto kA = nearest.kA;
+
+    auto kP = lemlib::linearInterp(target, x1, y1.kP, x2, y2.kP);
+    auto kI = lemlib::linearInterp(target, x1, y1.kI, x2, y2.kI);
+    auto kD = lemlib::linearInterp(target, x1, y1.kD, x2, y2.kD);
+
+    return Gains {kF, kA, kP, kI, kD};
+}
+
 /**
  * @brief Update the FAPID
  *
@@ -82,10 +183,23 @@ void FAPID::setExit(float largeError, float smallError, int largeTime, int small
  * PIDs could slow down the program.
  * @return float - output
  */
-float FAPID::update(float target, float position, bool log) {
+float lemlib::FAPID::update(float target, float position, bool log) {
+    // Check if we have scheduled gains, and if we have a different target
+    if (!scheduledGains.empty() && previousTarget != target) {
+        auto upper = std::upper_bound(scheduledGains.begin(), scheduledGains.end(), target,
+                                      [](auto value, auto entry) { return value < entry.first; });
+
+        // Nearest scheduled gains above and below (or equal) the target
+        auto above = *upper;
+        auto below = upper == scheduledGains.begin() ? *upper : *(--upper);
+        currentGains = gainInterpolator(target, below, above);
+    }
+
     // check most recent input if logging is enabled
     // this does not run by default because the mutexes could slow down the program
     // calculate output
+
+    auto [kF, kA, kP, kI, kD] = currentGains;
     float error = target - position;
     float deltaError = error - this->prevError;
     float output = this->kF * target + this->kP * error + this->kI * this->totalError + this->kD * deltaError;
@@ -178,32 +292,32 @@ void FAPID::log() {
             if (input == "reset()") {
                 reset();
             } else if (input == "kF") {
-                std::cout << this->kF << std::endl;
+                std::cout << this->currentGains.kF << std::endl;
             } else if (input == "kA") {
-                std::cout << this->kA << std::endl;
+                std::cout << this->currentGains.kA << std::endl;
             } else if (input == "kP") {
-                std::cout << this->kP << std::endl;
+                std::cout << this->currentGains.kP << std::endl;
             } else if (input == "kI") {
-                std::cout << this->kI << std::endl;
+                std::cout << this->currentGains.kI << std::endl;
             } else if (input == "kD") {
-                std::cout << this->kD << std::endl;
+                std::cout << this->currentGains.kD << std::endl;
             } else if (input == "totalError") {
                 std::cout << this->totalError << std::endl;
             } else if (input.find("kF_") == 0) {
                 input.erase(0, 3);
-                this->kF = std::stof(input);
+                this->currentGains.kF = std::stof(input);
             } else if (input.find("kA_") == 0) {
                 input.erase(0, 3);
-                this->kA = std::stof(input);
+                this->currentGains.kA = std::stof(input);
             } else if (input.find("kP_") == 0) {
                 input.erase(0, 3);
-                this->kP = std::stof(input);
+                this-currentGains.>kP = std::stof(input);
             } else if (input.find("kI_") == 0) {
                 input.erase(0, 3);
-                this->kI = std::stof(input);
+                this->currentGains.kI = std::stof(input);
             } else if (input.find("kD_") == 0) {
                 input.erase(0, 3);
-                this->kD = std::stof(input);
+                this->currentGains.kD = std::stof(input);
             }
             // clear the input
             input = "";