diff --git a/include/controllers/pi_controller.hpp b/include/controllers/pi_controller.hpp index fe83bf9..49229ac 100644 --- a/include/controllers/pi_controller.hpp +++ b/include/controllers/pi_controller.hpp @@ -2,19 +2,19 @@ namespace controllers { -struct pi_config +struct PIConfig { float Kp, Ki, Ts, upper_limit, lower_limit; }; -class pi_controller +class PIController { float error_sum, output, error_prev; float reset(float input, float output, float upper_limit, float lower_limit); float clamp(float input, float upper_limit, float lower_limit); public: - pi_controller(); - float loop(float error, const pi_config& config); + PIController(); + float loop(float error, const PIConfig& config, float external_integral = 0, bool reset_integral = false); }; } // namespace controllers diff --git a/include/controllers/pid_controller.hpp b/include/controllers/pid_controller.hpp index d212475..ae8620c 100644 --- a/include/controllers/pid_controller.hpp +++ b/include/controllers/pid_controller.hpp @@ -16,6 +16,6 @@ class pid_controller public: pid_controller(); - float loop(float error, const pid_config& config); + float loop(float error, const pid_config& config, float external_integral = 0, bool reset_integral = false); }; } // namespace controllers diff --git a/include/math/functions.hpp b/include/math/functions.hpp index e2681f5..8d36d60 100644 --- a/include/math/functions.hpp +++ b/include/math/functions.hpp @@ -6,7 +6,7 @@ namespace math { float abs(float input); float pow(float input, int power); -float wrap_angle(float input); +float wrapAngle(float input); float sin(float angle); float cos(float angle); float atan(float input); diff --git a/include/observers/bemf_observer.hpp b/include/observers/bemf_observer.hpp index c875b2f..021d957 100644 --- a/include/observers/bemf_observer.hpp +++ b/include/observers/bemf_observer.hpp @@ -12,7 +12,7 @@ class bemf_observer position_tracker tracker; float speed, angle, Ts; public: - bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts); + bemf_observer(const BemfGains& gains, const controllers::PIConfig& update_config, const controllers::PIConfig& tracker_config, float Ts); float loop(math::frame_abc line_currents, math::frame_abc line_voltages); }; } // namespace observers diff --git a/include/observers/dq_update.hpp b/include/observers/dq_update.hpp index 54b7f0e..bf45f71 100644 --- a/include/observers/dq_update.hpp +++ b/include/observers/dq_update.hpp @@ -5,10 +5,31 @@ namespace observers { -struct bemf_gains + +struct SetBemfParams +{ + bool error_d, error_sum_d, id_est, error_q, error_sum_q, iq_est; + SetBemfParams() : error_d(false), error_sum_d(false), id_est(false), error_q(false), error_sum_q(false), iq_est(false) {} + SetBemfParams(bool error_d, bool error_sum_d, bool id_est, bool error_q, bool error_sum_q, bool iq_est) + : error_d(error_d), error_sum_d(error_sum_d), id_est(id_est), error_q(error_q), error_sum_q(error_sum_q), iq_est(iq_est) + { + } +}; + +struct ExtBemfParams +{ + float error_d, error_sum_d, id_est, error_q, error_sum_q, iq_est; + ExtBemfParams() : error_d(0), error_sum_d(0), id_est(0), error_q(0), error_sum_q(0), iq_est(0) {} + ExtBemfParams(float error_d, float error_sum_d, float id_est, float error_q, float error_sum_q, float iq_est) + : error_d(error_d), error_sum_d(error_sum_d), id_est(id_est), error_q(error_q), error_sum_q(error_sum_q), iq_est(iq_est) + { + } +}; + +struct BemfGains { float VOLTAGE_GAIN, CURRENT_GAIN, EMF_GAIN, SPEED_CURRENT_GAIN; - bemf_gains(float Ld, float Lq, float Rs, float Ts, int axis) + BemfGains(float Ld, float Lq, float Rs, float Ts, int axis) { if(axis == 0) { VOLTAGE_GAIN = Ts/(2*Ld + Rs*Ts); @@ -29,12 +50,12 @@ class bemf_solver { float angular_velocity, rotor_angle, Ts; math::frame_dq I, V, X, E, I_prev, X_prev; - const bemf_gains& gains; - controllers::pi_config update_config; - controllers::pi_controller d_axis, q_axis; + const BemfGains& gains; + controllers::PIConfig update_config; + controllers::PIController d_axis, q_axis; public: - bemf_solver(const controllers::pi_config& config, const bemf_gains& gains, float Ts); - float loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity, float rotor_angle); + bemf_solver(const controllers::PIConfig& config, const BemfGains& gains, float Ts); + float loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity, float rotor_angle, const SetBemfParams& set_params = SetBemfParams(), const ExtBemfParams& ext_params = ExtBemfParams()); }; } // namespace observers diff --git a/include/observers/tracker.hpp b/include/observers/tracker.hpp index 99152b4..8ab7872 100644 --- a/include/observers/tracker.hpp +++ b/include/observers/tracker.hpp @@ -6,16 +6,30 @@ namespace observers { +struct SetTrackerParams +{ + bool error, speed, etheta; + SetTrackerParams() : error(false), speed(false), etheta(false) {} + SetTrackerParams(bool error, bool speed, bool etheta) : error(error), speed(speed), etheta(etheta) {} +}; + +struct ExtTrackerParams +{ + float error, speed, etheta; + ExtTrackerParams() : error(0), speed(0), etheta(0) {} + ExtTrackerParams(float error, float speed, float etheta) : error(error), speed(speed), etheta(etheta) {} +}; + class position_tracker { float speed_est, angle_est, Ts; - controllers::pi_controller angle_controller; - controllers::pi_config tracker_config; + controllers::PIController angle_controller; + controllers::PIConfig tracker_config; math::integrator angle_integrator; public: - position_tracker(controllers::pi_config config, float Ts); - float loop(float phase_error); + position_tracker(controllers::PIConfig config, float Ts); + float loop(float phase_error, const SetTrackerParams& params = SetTrackerParams(), const ExtTrackerParams& ext_params = ExtTrackerParams()); float speed_tracker(float angle_est); }; } // namespace observers diff --git a/src/controllers/pi_controller.cpp b/src/controllers/pi_controller.cpp index aec3ca2..3ffa598 100644 --- a/src/controllers/pi_controller.cpp +++ b/src/controllers/pi_controller.cpp @@ -1,10 +1,10 @@ #include -controllers::pi_controller::pi_controller() : error_sum(0), output(0), error_prev(0) +controllers::PIController::PIController() : error_sum(0), output(0), error_prev(0) { } -float controllers::pi_controller::clamp(float input, float upper_limit, float lower_limit) +float controllers::PIController::clamp(float input, float upper_limit, float lower_limit) { if (input < lower_limit) return lower_limit; @@ -14,7 +14,7 @@ float controllers::pi_controller::clamp(float input, float upper_limit, float lo return input; } -float controllers::pi_controller::reset(float input, float output, float upper_limit, float lower_limit) +float controllers::PIController::reset(float input, float output, float upper_limit, float lower_limit) { if (input > 0 && output >= upper_limit) return 0; @@ -24,14 +24,17 @@ float controllers::pi_controller::reset(float input, float output, float upper_l return input; } -float controllers::pi_controller::loop(float error, const pi_config& config) +float controllers::PIController::loop(float error, const PIConfig& config, float external_integral, bool reset_integral) { if (error > 0 && output >= config.upper_limit) error = 0; else if (error < 0 && output <= config.lower_limit) error = 0; - error_sum += clamp(config.Ki * config.Ts * (error + error_prev) / 2, config.upper_limit, config.lower_limit); + error_sum += config.Ki * config.Ts * (error + error_prev) / 2; + if (reset_integral) + error_sum = external_integral; + error_sum = clamp(error_sum, config.upper_limit, config.lower_limit); output = clamp(config.Kp * error + config.Ki * error_sum, config.upper_limit, config.lower_limit); error_prev = error; diff --git a/src/controllers/pid_controller.cpp b/src/controllers/pid_controller.cpp index 03836a7..d45d366 100644 --- a/src/controllers/pid_controller.cpp +++ b/src/controllers/pid_controller.cpp @@ -24,14 +24,17 @@ float controllers::pid_controller::reset(float input, float output, float upper_ return input; } -float controllers::pid_controller::loop(float error, const pid_config& config) +float controllers::pid_controller::loop(float error, const pid_config& config, float external_integral, bool reset_integral) { if (error > 0 && output >= config.upper_limit) error = 0; else if (error < 0 && output <= config.lower_limit) error = 0; - error_sum += clamp(config.Ki * config.Ts * (error + error_prev) / 2, config.upper_limit, config.lower_limit); + error_sum += config.Ki * config.Ts * (error + error_prev) / 2; + if (reset_integral) + error_sum = external_integral; + error_sum = clamp(error_sum, config.upper_limit, config.lower_limit); float derror = (error - error_prev) / config.Ts; output = clamp(config.Kp * error + config.Ki * error_sum + config.Kd * derror, config.upper_limit, config.lower_limit); diff --git a/src/math/functions.cpp b/src/math/functions.cpp index dce8629..e73c137 100644 --- a/src/math/functions.cpp +++ b/src/math/functions.cpp @@ -16,7 +16,7 @@ float math::pow(float input, int power) return output; } -float math::wrap_angle(float input) +float math::wrapAngle(float input) { if (input < -PI) return input + PI; diff --git a/src/observers/bemf_observer.cpp b/src/observers/bemf_observer.cpp index b503197..46740dc 100644 --- a/src/observers/bemf_observer.cpp +++ b/src/observers/bemf_observer.cpp @@ -1,6 +1,6 @@ #include -observers::bemf_observer::bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts) +observers::bemf_observer::bemf_observer(const BemfGains& gains, const controllers::PIConfig& update_config, const controllers::PIConfig& tracker_config, float Ts) : dq_update(update_config, gains, Ts), tracker(tracker_config, Ts), Ts(Ts) { } diff --git a/src/observers/dq_update.cpp b/src/observers/dq_update.cpp index 0acdb7a..dc2fcd1 100644 --- a/src/observers/dq_update.cpp +++ b/src/observers/dq_update.cpp @@ -1,7 +1,7 @@ #include #include -observers::bemf_solver::bemf_solver(const controllers::pi_config& config, const bemf_gains& gains, float Ts) +observers::bemf_solver::bemf_solver(const controllers::PIConfig& config, const BemfGains& gains, float Ts) : angular_velocity(0) , rotor_angle(0) , Ts(Ts) @@ -15,22 +15,38 @@ observers::bemf_solver::bemf_solver(const controllers::pi_config& config, const } float observers::bemf_solver::loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity, - float rotor_angle) + float rotor_angle, const SetBemfParams& set_params, const ExtBemfParams& ext_params) { + math::frame_dq I_est; + math::frame_dq error; + I = math::park_transform(math::clarke_transform(line_currents), rotor_angle); V = math::park_transform(math::clarke_transform(line_voltages), rotor_angle); - E.d = q_axis.loop(I.d - I_prev.d, update_config); - E.q = d_axis.loop(I.q - I_prev.q, update_config); - X.d = gains.VOLTAGE_GAIN * V.d + gains.SPEED_CURRENT_GAIN * I.q * angular_velocity + gains.EMF_GAIN * E.d; X.q = gains.VOLTAGE_GAIN * V.q + gains.SPEED_CURRENT_GAIN * I.d * angular_velocity + gains.EMF_GAIN * E.q; - I.d = X.d + X_prev.d + gains.CURRENT_GAIN * I_prev.d; - I.q = X.q + X_prev.q + gains.CURRENT_GAIN * I_prev.q; + I_est.d = X.d + X_prev.d + gains.CURRENT_GAIN * I_prev.d; + I_est.q = X.q + X_prev.q + gains.CURRENT_GAIN * I_prev.q; + + if(set_params.id_est) + I_est.d = ext_params.id_est; + if(set_params.iq_est) + I_est.q = ext_params.iq_est; + + error.d = I_est.d - I.d; + error.q = I_est.q - I.q; + + if(set_params.error_d) + error.d = ext_params.error_d; + if(set_params.error_q) + error.q = ext_params.error_q; + + E.d = q_axis.loop(error.d, update_config, set_params.error_sum_d, ext_params.error_sum_d); + E.q = d_axis.loop(error.q, update_config, set_params.error_sum_q, ext_params.error_sum_q); X_prev = X; - I_prev = I; + I_prev = I_est; - return math::atan2(E.q, E.d); + return math::atan2(E.d, E.q); } diff --git a/src/observers/tracker.cpp b/src/observers/tracker.cpp index 0772310..2cf9843 100644 --- a/src/observers/tracker.cpp +++ b/src/observers/tracker.cpp @@ -1,14 +1,19 @@ #include -observers::position_tracker::position_tracker(controllers::pi_config config, float Ts) +observers::position_tracker::position_tracker(controllers::PIConfig config, float Ts) : speed_est(0), angle_est(0), Ts(Ts), tracker_config(config) { } -float observers::position_tracker::loop(float phase_error) +float observers::position_tracker::loop(float phase_error, const SetTrackerParams& set_params, const ExtTrackerParams& ext_params) { - angle_est += Ts * angle_controller.loop(phase_error, tracker_config); - return math::wrap_angle(angle_est); + if(set_params.error) + phase_error = ext_params.error; + angle_est += Ts * angle_controller.loop(phase_error, tracker_config, set_params.speed, ext_params.speed); + if(set_params.etheta) + angle_est = ext_params.etheta; + angle_est = math::wrapAngle(angle_est); + return angle_est; } float observers::position_tracker::speed_tracker(float angle_est) diff --git a/tests/bemf_observer_test.cpp b/tests/bemf_observer_test.cpp index 99e5b4c..d52ca3b 100644 --- a/tests/bemf_observer_test.cpp +++ b/tests/bemf_observer_test.cpp @@ -8,9 +8,9 @@ int main() { - observers::bemf_gains gains(1, 1, 1, Ts, 0); - controllers::pi_config update_config = {1, 1, Ts, 180, -180}; - controllers::pi_config tracker_config = {1, 1, Ts, 180,-180}; + observers::BemfGains gains(1, 1, 1, Ts, 0); + controllers::PIConfig update_config = {1, 1, Ts, 180, -180}; + controllers::PIConfig tracker_config = {1, 1, Ts, 180,-180}; observers::bemf_observer bemf_observer(gains, update_config, tracker_config, Ts); math::frame_abc line_currents; diff --git a/tests/pi_test.cpp b/tests/pi_test.cpp index 09681fa..c75bae6 100644 --- a/tests/pi_test.cpp +++ b/tests/pi_test.cpp @@ -8,8 +8,8 @@ using namespace controllers; int main() { float x_meas = 0, x_ref = 10; - pi_config config = { 2, 1, Ts, 10, -10 }; - pi_controller controller; + PIConfig config = { 2, 1, Ts, 10, -10 }; + PIController controller; std::cout << " Starting loop" << std::endl; for (float t = 0; t < 10; t += Ts) {