53 lines
1.5 KiB
C++
53 lines
1.5 KiB
C++
// This is a personal academic project. Dear PVS-Studio, please check it.
|
|
// PVS-Studio Static Code Analyzer for C, C++, C#, and Java: https://pvs-studio.com
|
|
|
|
#include "Motor/BrushedMotor.hpp"
|
|
#include "SavedPidRegulator.hpp"
|
|
#include "esp_log.h"
|
|
|
|
|
|
class PIDController : public SavedPidRegulator {
|
|
public:
|
|
PIDController(float p, float i, float d, BrushedMotor *rotor,
|
|
const char *name = "PID", uint16_t dt = 10, const bool isEnabled = false)
|
|
: SavedPidRegulator(p, i, d, name, dt) {
|
|
assert(rotor != nullptr);
|
|
_rotor = rotor;
|
|
_isEnabled = isEnabled;
|
|
setLimits(-_rotor->maxDuty(), _rotor->maxDuty());
|
|
setDirection(REVERSE);
|
|
_pid_timer = millis();
|
|
ESP_LOGI(_tag, "PID %s initialized with params (%f, %f, %f)", name, Kp, Ki, Kd);
|
|
}
|
|
|
|
void enable() {
|
|
_isEnabled = true;
|
|
_rotor->setDuty(0);
|
|
integral = 0;
|
|
}
|
|
|
|
void disable() {
|
|
_isEnabled = false;
|
|
_rotor->setDuty(0);
|
|
}
|
|
|
|
void update() {
|
|
if (_isEnabled) {
|
|
//ESP_LOGI(_tag, "Setpoint: %f", setpoint);
|
|
//ESP_LOGI(_tag, "Input: %f", input);
|
|
//ESP_LOGI(_tag, "Output: %d", (int16_t) round(getResultNow()));
|
|
setRotorDuty((int16_t) round(getResultNow()));
|
|
}
|
|
}
|
|
|
|
void setRotorDuty(int16_t duty) {
|
|
/* Used only for test purposes */
|
|
_rotor->setExtendedDuty(duty);
|
|
}
|
|
|
|
private:
|
|
//uint16_t _dt;
|
|
BrushedMotor *_rotor = nullptr;
|
|
uint32_t _pid_timer = 0;
|
|
bool _isEnabled = false;
|
|
}; |