diff --git a/include/lemlib/pidtuner.hpp b/include/lemlib/pidtuner.hpp new file mode 100644 index 000000000..7acea7573 --- /dev/null +++ b/include/lemlib/pidtuner.hpp @@ -0,0 +1,45 @@ +#include "lemlib/pid.hpp" +#include "pros/motor_group.hpp" +#include "pros/motors.hpp" +#include + +namespace lemlib { + +enum class TUNER_TARGET { distance = 0, turning = 1, flywheel = 2, catapult = 3, misc = 4 }; + +class PIDTuner { + private: + Gains gains; + std::shared_ptr pid; + + TUNER_TARGET target; + + std::shared_ptr> 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& pid, std::shared_ptr>& motorGroups, + TUNER_TARGET target, bool tunekF, bool tunekA, bool tunekP, bool tunekI, bool tunekD); + + ~PIDTuner(); + + std::shared_ptr tunePID(); +}; + +} // namespace lemlib \ No newline at end of file diff --git a/src/lemlib/pidtuner.cpp b/src/lemlib/pidtuner.cpp new file mode 100644 index 000000000..3c8890524 --- /dev/null +++ b/src/lemlib/pidtuner.cpp @@ -0,0 +1,245 @@ +#include "lemlib/pidtuner.hpp" +#include "pros/motor_group.hpp" +#include +#include + +namespace lemlib { + +PIDTuner::PIDTuner(std::shared_ptr& pidArg, std::shared_ptr>& 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 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 dataPoints; + std::vector> boxesSizeA; + std::vector> boxesSizeB; + std::vector> boxesSizeC; + std::vector> 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 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 \ No newline at end of file