diff --git a/src/propulsion/main.cpp b/src/propulsion/main.cpp index a4de79a4..486661b7 100644 --- a/src/propulsion/main.cpp +++ b/src/propulsion/main.cpp @@ -1,5 +1,7 @@ #include "main.hpp" +#include "utils/math/regression.hpp" + namespace hyped::propulsion { Main::Main() @@ -53,6 +55,14 @@ void Main::run() break; case data::State::kCalibrating: if (state_processor_.isInitialised()) { + // set coefficients for rpm regulator + utils::math::Regression regression; + // TODO:find actual x and y data (and probably get from json or something) + regression.CaulculateCoefficients({1, 2, 3}, {1, 2, 3}); + std::vector coefficients = regression.coefficients_; + RpmRegulator rpm_regulator; + rpm_regulator.setCoefficients(coefficients); + if (motor_data.module_status != data::ModuleStatus::kReady) { motor_data.module_status = data::ModuleStatus::kReady; data.setMotorData(motor_data); diff --git a/src/propulsion/rpm_regulator.cpp b/src/propulsion/rpm_regulator.cpp index 06dcd1d8..116edbc4 100644 --- a/src/propulsion/rpm_regulator.cpp +++ b/src/propulsion/rpm_regulator.cpp @@ -2,6 +2,8 @@ #include +#include "utils/math/regression.hpp" + namespace hyped::propulsion { RpmRegulator::RpmRegulator() @@ -16,11 +18,19 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3 return std::max(target, 0); } +void RpmRegulator::setCoefficients(std::vector coefficients) +{ + coefficients_ = coefficients; +} + int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity) { + double beta1 = coefficients_.at(0); + double beta0 = coefficients_.at(1); + // polynomial values from simulation - return std::round(0.32047 * actual_velocity * actual_velocity + 297.72578 * actual_velocity - + 1024.30824); + return std::round(beta1 * actual_velocity + beta0); + return 0; } int32_t RpmRegulator::step(const int32_t optimal_rpm, const int32_t actual_rpm) diff --git a/src/propulsion/rpm_regulator.hpp b/src/propulsion/rpm_regulator.hpp index d76fd18e..fa18b442 100644 --- a/src/propulsion/rpm_regulator.hpp +++ b/src/propulsion/rpm_regulator.hpp @@ -16,6 +16,14 @@ class RpmRegulator { */ static int32_t calculateRpm(const data::nav_t actual_velocity, const int32_t actual_rpm); + /** + * @brief sets the coefficients calculated on startup in utils to be stored + * as private class members. + * + * @param actual_velocity + */ + void setCoefficients(std::vector coeffients); + private: /* * @brief Construct a new rpm regulator object @@ -36,7 +44,11 @@ class RpmRegulator { * @param actual_rpm - the rpm of the motor * @return int32_t - the step with which to increase the rpm */ - static int32_t step(const int32_t optimal_rpm, const int32_t actual_rpm); + int32_t step(const int32_t optimal_rpm, const int32_t actual_rpm); + + // coefficients vector of form {beta1, beta0} + std::vector coefficients_; + }; } // namespace hyped::propulsion diff --git a/src/utils/io/spi.cpp b/src/utils/io/spi.cpp index 5f721d6b..43252e3d 100644 --- a/src/utils/io/spi.cpp +++ b/src/utils/io/spi.cpp @@ -266,6 +266,6 @@ Spi::~Spi() close(spi_fd_); } -} // namespace io } // namespace utils } // namespace hyped +} // namespace hyped diff --git a/src/utils/math/regression.cpp b/src/utils/math/regression.cpp new file mode 100644 index 00000000..49de3f11 --- /dev/null +++ b/src/utils/math/regression.cpp @@ -0,0 +1,37 @@ +#include "regression.hpp" + +namespace hyped::utils::math { + +Regression::Regression() +{ +} + +void Regression::CaulculateCoefficients(const std::vector x_data, + const std::vector y_data) +{ + double x_sum = 0; + double y_sum = 0; + + for (size_t i = 0; i < x_data.size(); ++i) { + x_sum += x_data.at(i); + y_sum += y_data.at(i); + } + + double x_mean = static_cast(x_sum / x_data.size()); + double y_mean = static_cast(y_sum / y_data.size()); + double s_xx = 0; + double s_yy = 0; + double s_xy = 0; + + for (size_t i = 0; i < x_data.size(); ++i) { + s_xx += std::pow(x_data.at(i) - x_mean, 2); + s_yy += std::pow(y_data.at(i) - y_mean, 2); + s_xy += (x_data.at(i) - x_mean) * (y_data.at(i) - y_mean); + } + // regressed function will have form y = beta1*x + beta0 + double beta1 = s_xy / s_xx; + double beta0 = y_mean - (beta1 - x_mean); + + coefficients_ = {beta1, beta0}; +} +} // namespace hyped::utils::math \ No newline at end of file diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp new file mode 100644 index 00000000..3828dfbe --- /dev/null +++ b/src/utils/math/regression.hpp @@ -0,0 +1,24 @@ +#pragma once +#include + +#include + +namespace hyped::utils::math { + +class Regression { + public: + Regression(); + + /** + * @brief calculates regressed coefficients using basic x-y linear regression. + * only works for straight lines! + * + * @param x_data vector of x-data points + * @param y_data vector of y-data points + */ + void CaulculateCoefficients(std::vector x_data, std::vector y_data); + + // calculated coefficients from regression function + std::vector coefficients_; +}; +} // namespace hyped::utils::math \ No newline at end of file