Skip to content

Commit

Permalink
Clang format
Browse files Browse the repository at this point in the history
  • Loading branch information
@sufferiing
sufferiing committed Nov 25, 2023
1 parent b104bb0 commit 262c98e
Show file tree
Hide file tree
Showing 3 changed files with 86 additions and 98 deletions.
55 changes: 27 additions & 28 deletions include/lemlib/pid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,40 +15,39 @@

namespace lemlib {
struct Gains {
float kF = 0;
float kA = 0;
float kP = 0;
float kI = 0;
float kD = 0;
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
*/
* @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
*/
* @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
*
Expand All @@ -69,7 +68,7 @@ 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
*
Expand All @@ -86,7 +85,7 @@ class FAPID {
* @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
*
Expand All @@ -96,7 +95,7 @@ class FAPID {
* @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
*
Expand Down Expand Up @@ -189,7 +188,8 @@ class FAPID {
// 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
// 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;
Expand All @@ -214,5 +214,4 @@ class FAPID {
static pros::Mutex logMutex;
};


} // namespace lemlib
124 changes: 58 additions & 66 deletions src/lemlib/pid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,38 +40,39 @@ lemlib::FAPID::FAPID(float kF, float kA, float kP, float kI, float kD, std::stri
}

/**
* @brief Construct a new FAPID
*
* @param gains the gains for the FAPID to use
* @param name name of the FAPID. Used for logging
*/
* @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
*/
* @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) {
* @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;
Expand All @@ -93,31 +94,25 @@ void lemlib::FAPID::setGains(float kF, float kA, float kP, float kI, float kD) {
}

/**
* @brief Set gains
*
* @param gains the new gains
*/
void lemlib::FAPID::setGains(Gains gains) {
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 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 gain interpolator
*
* @param gains the new gain interpolator
*/
void lemlib::FAPID::setGainInterpolator(Interpolator interpolator) { gainInterpolator = interpolator; }

/**
* @brief Set the exit conditions
Expand All @@ -137,37 +132,37 @@ void FAPID::setExit(float largeError, float smallError, int largeTime, int small
}

/**
* @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
*/
* @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)) {
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
*/
* @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?
// Should kF and kA be interpolated?
// It seems that feedforward constants should remain the same

auto kF = nearest.kF;
Expand All @@ -176,8 +171,8 @@ lemlib::Gains interpolateLinear(float target, std::pair<float, Gains> below, std
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 };

return Gains {kF, kA, kP, kI, kD};
}

/**
Expand All @@ -192,12 +187,9 @@ lemlib::Gains interpolateLinear(float target, std::pair<float, Gains> below, std
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; });

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);
Expand Down
5 changes: 1 addition & 4 deletions src/lemlib/util.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -195,12 +195,9 @@ Pose circleLineIntersect(Pose p1, Pose p2, Pose center, float radius) {
* @return the interpolated y value
*/
float linearInterp(float x, float x1, float y1, float x2, float y2) {
if (x2 - x1 == 0) {
return y1;
}
if (x2 - x1 == 0) { return y1; }

return y1 + (x - x1) * ((y2 - y1) / (x2 - x1));
}

}; // namespace lemlib

0 comments on commit 262c98e

Please sign in to comment.