Skip to content

Commit

Permalink
Merge branch 'PID-Tuner' into refactor-master
Browse files Browse the repository at this point in the history
  • Loading branch information
UvuvDev committed Mar 13, 2024
2 parents c9be17b + acaab08 commit e55d665
Show file tree
Hide file tree
Showing 2 changed files with 290 additions and 0 deletions.
45 changes: 45 additions & 0 deletions include/lemlib/pidtuner.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,45 @@
#include "lemlib/pid.hpp"
#include "pros/motor_group.hpp"
#include "pros/motors.hpp"
#include <vector>

namespace lemlib {

enum class TUNER_TARGET { distance = 0, turning = 1, flywheel = 2, catapult = 3, misc = 4 };

class PIDTuner {
private:
Gains gains;
std::shared_ptr<FAPID> pid;

TUNER_TARGET target;

std::shared_ptr<std::vector<pros::MotorGroup>> motorGroups;

void tuneFlywheel(int targetRPM, float gearRatio);
void tuneCatapult(int targetRPM);
void tuneDistance(float targetDistance);
void tuneTurning(float targetAngle);
void tuneMisc(float target);
public:
/**
* @brief Construct a new PIDTuner object
*
* @param pidArg The PID controller to tune the constants in
* @param motorGroupArg The motor group that is being tuned
* @param target The Target System that is being tuned
* @param tunekF Whether to tune the Feed Forward Constant (kF)
* @param tunekA Whether to tune the Acceleration Gain Constant (kA)
* @param tunekP Whether to tune the Proportional Gain Constant (kP)
* @param tunekI Whether to tune the Integral Gain Constant (kI)
* @param tunekD Whether to tune the Derivative Gain Constant (kD)
*/
PIDTuner(std::shared_ptr<FAPID>& pid, std::shared_ptr<std::vector<pros::MotorGroup>>& motorGroups,
TUNER_TARGET target, bool tunekF, bool tunekA, bool tunekP, bool tunekI, bool tunekD);

~PIDTuner();

std::shared_ptr<FAPID> tunePID();
};

} // namespace lemlib
245 changes: 245 additions & 0 deletions src/lemlib/pidtuner.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,245 @@
#include "lemlib/pidtuner.hpp"
#include "pros/motor_group.hpp"
#include <cmath>
#include <memory>

namespace lemlib {

PIDTuner::PIDTuner(std::shared_ptr<FAPID>& pidArg, std::shared_ptr<std::vector<pros::MotorGroup>>& motorGroups,
TUNER_TARGET target, bool tunekF, bool tunekA, bool tunekP, bool tunekI, bool tunekD) {
gains = {(float)tunekF, (float)tunekA, (float)tunekP, (float)tunekI, (float)tunekD};

this->pid = pidArg;

this->target = target;

this->pid->setGains(gains.kF, gains.kA, gains.kP, gains.kI, gains.kD);

this->motorGroups = motorGroups;
}

void PIDTuner::tuneFlywheel(int targetRPM, float gearRatio) {
float avgVelocity = 0;

std::vector<float> filteredVelocity;

if (gains.kF != 0) {
// While kF's value is not reaching around 100 RPM of the target RPM and its not overheating
while ((avgVelocity < targetRPM - 200 || avgVelocity > targetRPM) &&
motorGroups->at(0).get_temperature(0) > 55) {
avgVelocity = 0; // Reset the average velocity to 0

for (int i = 0; i > 5; i++) {
// Average out the velocity of all the motors for past 5 runs
for (int i = 0; i < motorGroups->size(); i++) {
avgVelocity += motorGroups->at(i).get_actual_velocity() * gearRatio;
avgVelocity /= motorGroups->size();
filteredVelocity.emplace_back(avgVelocity);
}
}

// Get the Feed Forward output
float motorVoltage = pid->update(targetRPM, avgVelocity);

// Move the motors at the calculated voltage
for (int i = 0; i < motorGroups->size(); i++) { motorGroups->at(i).move_voltage(motorVoltage); }

// Depending on the current kF value, adjust it accordingly
if (avgVelocity > targetRPM - 200) {
gains.kF -= .1;
pros::delay(1000);
} else if (avgVelocity < targetRPM - 300) {
gains.kF *= 2;
pros::delay(3000);
} else {
gains.kF += .1;
pros::delay(1000);
}

pros::delay(20);
}
}

// kF is done!

if (gains.kP != 0) {
// While the average velocity is outside of 5 RPM of the target RPM
while ((avgVelocity > targetRPM + 5 || avgVelocity < targetRPM - 5) &&
motorGroups->at(0).get_temperature(0) > 55) {
avgVelocity = 0; // Reset the average velocity to 0

// Average out the velocity of all the motors
for (int i = 0; i < motorGroups->size(); i++) {
avgVelocity += motorGroups->at(i).get_actual_velocity() * gearRatio;
avgVelocity /= motorGroups->size();
}

// Get the Feed Forward output
float motorVoltage = pid->update(targetRPM, avgVelocity);

// Move the motors at the calculated voltage
for (int i = 0; i < motorGroups->size(); i++) { motorGroups->at(i).move_voltage(motorVoltage); }

// Depending on the current kF value, adjust it accordingly
if (avgVelocity > targetRPM - 50) {
gains.kF -= .1;
pros::delay(1000);
} else if (avgVelocity < targetRPM - 300) {
gains.kF *= 2;
pros::delay(3000);
} else {
gains.kF += .1;
pros::delay(1000);
}

pros::delay(20);
}
}

// kP is done!

/*
* While kP average velocity goes to within 5 RPM of the target value
*
*
*
*
*/

if (gains.kD != 0) {
// While the average velocity is outside of 5 RPM of the target RPM
std::vector<float> dataPoints;
std::vector<std::vector<bool>> boxesSizeA;
std::vector<std::vector<bool>> boxesSizeB;
std::vector<std::vector<bool>> boxesSizeC;
std::vector<std::vector<bool>> boxesSizeD;

float dimensions = 1;

while ((avgVelocity > targetRPM + 5 || avgVelocity < targetRPM - 5) &&
motorGroups->at(0).get_temperature(0) > 55) {
dataPoints.resize(500);

boxesSizeA.resize(dataPoints.size());
boxesSizeB.resize(dataPoints.size());
boxesSizeC.resize(dataPoints.size());
boxesSizeD.resize(dataPoints.size());

float AboxW, AboxH = 3.0f;
float BboxW, BboxH = 2.0f;
float CboxW, CboxH = 1.0f;
float DboxW, DboxH = 0.5f;

float gridX, gridY = (float)motorGroups->at(0).get_gearing() * gearRatio;

dataPoints[0] = motorGroups->at(0).get_actual_velocity() * gearRatio;

// Find the largest boxes.
for (int i = 0; i < boxesSizeA.size(); i++) {
float leftBox = AboxW * i;
float rightBox = AboxW * (i + 1);
float topBox = AboxH * i;
float bottomBox = AboxH * (i + 1);

for (int j = 0; j < dataPoints.size(); j++) {
// If the datapoint is between the top and bottom of the box
if (dataPoints.at(j) < topBox && dataPoints.at(j) > bottomBox) {
boxesSizeA.at(i).at(j) = true; // Box is filled.
} else {
boxesSizeA.at(i).at(j) = false; // Box is not filled.
}
}
}

// Find the second largest boxes.
for (int i = 0; i < boxesSizeB.size(); i++) {
float leftBox = BboxW * i;
float rightBox = BboxW * (i + 1);
float topBox = BboxH * i;
float bottomBox = BboxH * (i + 1);

for (int j = 0; j < dataPoints.size(); j++) {
// If the datapoint is between the top and bottom of the box
if (dataPoints.at(j) < topBox && dataPoints.at(j) > bottomBox) {
boxesSizeB.at(i).at(j) = true; // Box is filled.
} else {
boxesSizeB.at(i).at(j) = false; // Box is not filled.
}
}
}

// Find the third largest boxes.
for (int i = 0; i < boxesSizeC.size(); i++) {
float leftBox = CboxW * i;
float rightBox = CboxW * (i + 1);
float topBox = CboxH * i;
float bottomBox = CboxH * (i + 1);

for (int j = 0; j < dataPoints.size(); j++) {
// If the datapoint is between the top and bottom of the box
if (dataPoints.at(j) < topBox && dataPoints.at(j) > bottomBox) {
boxesSizeC.at(i).at(j) = true; // Box is filled.
} else {
boxesSizeC.at(i).at(j) = false; // Box is not filled.
}
}
}

// Find the smallest boxes.
for (int i = 0; i < boxesSizeD.size(); i++) {
float leftBox = DboxW * i;
float rightBox = DboxW * (i + 1);
float topBox = DboxH * i;
float bottomBox = DboxH * (i + 1);

for (int j = 0; j < dataPoints.size(); j++) {
// If the datapoint is between the top and bottom of the box
if (dataPoints.at(j) < topBox && dataPoints.at(j) > bottomBox) {
boxesSizeD.at(i).at(j) = true; // Box is filled.
} else {
boxesSizeD.at(i).at(j) = false; // Box is not filled.
}
}
}

float dimensionsA = std::log(1 / boxesSizeA.size()) / std::log(1 / AboxH);
float dimensionsB = std::log(1 / boxesSizeB.size()) / std::log(1 / BboxH);
float dimensionsC = std::log(1 / boxesSizeC.size()) / std::log(1 / CboxH);
float dimensionsD = std::log(1 / boxesSizeD.size()) / std::log(1 / DboxH);

static constexpr float flatDimension = 2.05f;
static constexpr float curvyDimension = 2.4f;

if (dimensionsA > curvyDimension && dimensionsB > curvyDimension && dimensionsC > curvyDimension &&
dimensionsD > curvyDimension) {
gains.kD *= 2;
} else if (dimensionsA < flatDimension && dimensionsB < flatDimension && dimensionsC < flatDimension &&
dimensionsD < flatDimension) {
gains.kD += .1;
} else {
break;
}
}
}
}

void PIDTuner::tuneCatapult(int targetRPM) {}

void PIDTuner::tuneDistance(float targetDistance) {}

void PIDTuner::tuneTurning(float targetAngle) {}

void PIDTuner::tuneMisc(float target) {}

std::shared_ptr<FAPID> PIDTuner::tunePID() {
switch (target) {
case TUNER_TARGET::flywheel: tuneFlywheel(2000, 1); break;
case TUNER_TARGET::catapult: break;
case TUNER_TARGET::distance: break;
case TUNER_TARGET::turning: break;
case TUNER_TARGET::misc: break;
}
return pid;
}

} // namespace lemlib

0 comments on commit e55d665

Please sign in to comment.