56 lines
1.8 KiB
C++
56 lines
1.8 KiB
C++
#include "GyverPID.h"
|
|
#include "Motor/BrushedMotor.hpp"
|
|
#include "Preferences.h"
|
|
#include "esp_log.h"
|
|
#include "mString.h"
|
|
|
|
class PIDController : public GyverPID {
|
|
public:
|
|
PIDController(float p, float i, float d, BrushedMotor *rotor, int rotorNumber = 1, uint16_t dt = 10)
|
|
: GyverPID(p, i, d, dt) {
|
|
assert(rotor != nullptr);
|
|
_rotor = rotor;
|
|
_rotorNumber = rotorNumber;
|
|
this->_dt = dt;
|
|
setLimits(0, _rotor->maxDuty());
|
|
_preferences.begin((String("r") + String(rotorNumber)).c_str());
|
|
_loadPreferences(p, i, d);
|
|
pid_timer = millis();
|
|
ESP_LOGI(_tag, "PID №%d initialized with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
|
|
}
|
|
|
|
void tick() {
|
|
if (millis() - pid_timer >= _dt) {
|
|
_rotor->setDuty(getResultTimer());
|
|
pid_timer = millis();
|
|
}
|
|
}
|
|
|
|
void setRotorDuty(uint32_t duty) {
|
|
/* Used only for test purposes */
|
|
_rotor->setDuty(duty);
|
|
}
|
|
|
|
void save() {
|
|
_preferences.putFloat("p", Kp);
|
|
_preferences.putFloat("i", Ki);
|
|
_preferences.putFloat("d", Kd);
|
|
ESP_LOGI(_tag, "PID №%d saved with params (%f, %f, %f)", _rotorNumber, Kp, Ki, Kd);
|
|
}
|
|
|
|
private:
|
|
Preferences _preferences;
|
|
int _rotorNumber = 1;
|
|
uint16_t _dt;
|
|
BrushedMotor *_rotor = nullptr;
|
|
uint32_t pid_timer = 0;
|
|
static constexpr const char *_tag = "PIDController";
|
|
|
|
void _loadPreferences(float pDefault = 0, float iDefault = 0, float dDefault = 0) {
|
|
if (_preferences.isKey("p") and _preferences.isKey("i") and _preferences.isKey("d")) {
|
|
Kp = _preferences.getFloat("p", pDefault);
|
|
Ki = _preferences.getFloat("i", iDefault);
|
|
Kd = _preferences.getFloat("d", dDefault);
|
|
}
|
|
}
|
|
}; |