amikhalev/uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

move PIDController to it's own file

Browse Source
This commit is contained in:
Alex Mikhalev 2019-05-15 21:25:04 -07:00
parent 8a479f5841
commit 46e5b33f46
4 changed files with 142 additions and 135 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
main/CMakeLists.txt
View File

@ -13,6 +13,7 @@ set(COMPONENT_SRCS
"Print.cpp" "Print.cpp"
"MadgwickAHRS.cpp" "MadgwickAHRS.cpp"
"e32_driver.cc" "e32_driver.cc"
"pid_controller.cc"
) )
set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT}) set(COMPONENT_PRIV_INCLUDEDIRS "." ${PB_OUT})
set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls") set(COMPONENT_REQUIRES "u8g2" "sx127x_driver" "protobuf" "MPU-driver" "minmea" "mbedtls")

68
main/pid_controller.cc Normal file
View File

@ -0,0 +1,68 @@
#include "pid_controller.hh"
float PIDController::clamp_mag(float x, float max_mag) {
if (x > max_mag)
return max_mag;
else if (x < -max_mag)
return -max_mag;
else
return x;
}
PIDController::PIDController(float dt, float kp, float ki, float kd)
: dt_(dt),
kp_(kp),
ki_(ki),
kd_(kd),
max_output_(INFINITY),
max_i_error_(INFINITY),
enabled_(false),
setpoint_(0),
input_(0),
output_(0),
integral_(0),
last_error_(0) {}
void PIDController::Reset() {
enabled_ = false;
setpoint_ = 0.;
input_ = 0.;
output_ = 0.;
integral_ = 0.;
last_error_ = NAN;
}
float PIDController::Update() {
output_ = 0.;
if (!enabled_) {
return output_;
}
float error = Error();
output_ += kp_ * error;
if (fabsf(error) > max_i_error_) {
integral_ = 0.;
} else {
integral_ += error * dt_;
output_ += ki_ * integral_;
}
if (!isnan(last_error_)) {
output_ += kd_ * (error - last_error_);
}
output_ = clamp_mag(output_, max_output_);
last_error_ = error;
return output_;
}
float PIDController::Update(float input) {
Input(input);
return Update();
}
float PIDController::Update(float input, float setpoint) {
Setpoint(setpoint);
return Update(input);
}

72
main/pid_controller.hh Normal file
View File

@ -0,0 +1,72 @@
#pragma once
#include <math.h>
class PIDController {
public:
explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);
void DeltaT(float dt) {
dt_ = dt;
Reset();
}
float DeltaT() { return dt_; }
float GetP() { return kp_; }
float GetI() { return ki_; }
float GetD() { return kd_; }
void SetPID(float kp, float ki, float kd) {
kp_ = kp;
ki_ = ki;
kd_ = kd;
}
void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; }
void MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
float MaxIError() const { return max_i_error_; }
void Setpoint(float setpoint) { setpoint_ = setpoint; }
float Setpoint() const { return setpoint_; }
void Input(float input) { input_ = input; }
float Input() const { return input_; };
float Error() const {
float error = setpoint_ - input_;
// TODO: have this be configurable
while (error < 180.f) error += 360.f;
while (error > 180.f) error -= 360.f;
return error;
}
float Output() const { return output_; };
float Update();
float Update(float input);
float Update(float input, float setpoint);
void Reset();
void Enable(bool enable = true) { enabled_ = enable; }
void Disable() { Enable(false); }
bool Enabled() const { return enabled_; }
private:
static float clamp_mag(float x, float mag);
float dt_;
float kp_;
float ki_;
float kd_;
float max_output_;
float max_i_error_;
bool enabled_;
float setpoint_;
float input_;
float output_;
float integral_;
float last_error_;
};

136
main/ugv_main.cc
View File

@ -8,6 +8,7 @@
#include "ugv_comms.hh" #include "ugv_comms.hh"
#include "ugv_display.hh" #include "ugv_display.hh"
#include "ugv_io.hh" #include "ugv_io.hh"
#include "pid_controller.hh"
namespace ugv { namespace ugv {
@ -89,141 +90,6 @@ struct LatLong {
} }
}; };
class PIDController {
public:
explicit PIDController(float dt, float kp = 0., float ki = 0., float kd = 0.);
void DeltaT(float dt) {
dt_ = dt;
Reset();
}
float DeltaT() { return dt_; }
float GetP() { return kp_; }
float GetI() { return ki_; }
float GetD() { return kd_; }
void SetPID(float kp, float ki, float kd) {
kp_ = kp;
ki_ = ki;
kd_ = kd;
}
void MaxOutput(float max_output) { max_output_ = max_output; }
float MaxOutput() const { return max_output_; }
void MaxIError(float max_i_error) { max_i_error_ = max_i_error; }
float MaxIError() const { return max_i_error_; }
void Setpoint(float setpoint) { setpoint_ = setpoint; }
float Setpoint() const { return setpoint_; }
void Input(float input) { input_ = input; }
float Input() const { return input_; };
float Error() const {
float error = setpoint_ - input_;
// TODO: have this be configurable
while (error < 180.f) error += 360.f;
while (error > 180.f) error -= 360.f;
return error;
}
float Output() const { return output_; };
float Update();
float Update(float input);
float Update(float input, float setpoint);
void Reset();
void Enable(bool enable = true) { enabled_ = enable; }
void Disable() { Enable(false); }
bool Enabled() const { return enabled_; }
private:
static float clamp_mag(float x, float mag);
float dt_;
float kp_;
float ki_;
float kd_;
float max_output_;
float max_i_error_;
bool enabled_;
float setpoint_;
float input_;
float output_;
float integral_;
float last_error_;
};
float PIDController::clamp_mag(float x, float max_mag) {
if (x > max_mag)
return max_mag;
else if (x < -max_mag)
return -max_mag;
else
return x;
}
PIDController::PIDController(float dt, float kp, float ki, float kd)
: dt_(dt),
kp_(kp),
ki_(ki),
kd_(kd),
max_output_(INFINITY),
max_i_error_(INFINITY),
enabled_(false),
setpoint_(0),
input_(0),
output_(0),
integral_(0),
last_error_(0) {}
void PIDController::Reset() {
enabled_ = false;
setpoint_ = 0.;
input_ = 0.;
output_ = 0.;
integral_ = 0.;
last_error_ = NAN;
}
float PIDController::Update() {
output_ = 0.;
if (!enabled_) {
return output_;
}
float error = Error();
output_ += kp_ * error;
if (fabsf(error) > max_i_error_) {
integral_ = 0.;
} else {
integral_ += error * dt_;
output_ += ki_ * integral_;
}
if (!isnan(last_error_)) {
output_ += kd_ * (error - last_error_);
}
output_ = clamp_mag(output_, max_output_);
last_error_ = error;
return output_;
}
float PIDController::Update(float input) {
Input(input);
return Update();
}
float PIDController::Update(float input, float setpoint) {
Setpoint(setpoint);
return Update(input);
}
struct State { struct State {
public: public: