From 32b496c899840d6c6fb527fb2e544d744f6ae21f Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Tue, 29 Mar 2022 17:24:55 +0100 Subject: [PATCH 1/6] Mtc: Optimal frequency regression --- src/propulsion/main.cpp | 3 ++ src/propulsion/rpm_regulator.cpp | 7 ++-- src/utils/io/spi.cpp | 4 +-- src/utils/math/regression.hpp | 58 ++++++++++++++++++++++++++++++++ 4 files changed, 68 insertions(+), 4 deletions(-) create mode 100644 src/utils/math/regression.hpp diff --git a/src/propulsion/main.cpp b/src/propulsion/main.cpp index 85771ae6..008d3396 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,7 @@ void Main::run() break; case data::State::kCalibrating: if (state_processor_.isInitialised()) { + hyped::utils::math::Regression::Regression(); 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..cf33d4d0 100644 --- a/src/propulsion/rpm_regulator.cpp +++ b/src/propulsion/rpm_regulator.cpp @@ -18,9 +18,12 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity) { + struct hyped::utils::math::Regression regression; + const double beta1 = regression.Coefficients.beta1; + const double beta0 = regression.Coefficients.beta0; + // 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); } int32_t RpmRegulator::step(const int32_t optimal_rpm, const int32_t actual_rpm) diff --git a/src/utils/io/spi.cpp b/src/utils/io/spi.cpp index aa41520a..3c694ae8 100644 --- a/src/utils/io/spi.cpp +++ b/src/utils/io/spi.cpp @@ -217,7 +217,7 @@ void SPI::transfer(uint8_t *tx, uint8_t *, uint16_t len) } #endif -} +} // namespace io void SPI::read(uint8_t addr, uint8_t *rx, uint16_t len) { @@ -266,6 +266,6 @@ SPI::~SPI() close(spi_fd_); } -} // namespace io } // namespace utils } // namespace hyped +} // namespace hyped diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp new file mode 100644 index 00000000..15325e87 --- /dev/null +++ b/src/utils/math/regression.hpp @@ -0,0 +1,58 @@ +#pragma once +#include + +#include + +namespace hyped::utils::math { + +class Regression { + public: + Regression(); + std::vector GetCoeffs(std::vector x_data, std::vector y_data); + + struct Coefficients { + const double beta0; + const double beta1; + } coefficients; + + private: + std::vector coefficients_; +}; + +Regression::Regression() +{ + coefficients_ = GetCoeffs(std::vector x_data, std::vector y_data); + coefficients.beta0 = Regression::coefficients_.at(1); + coefficients.beta1 = Regression::coefficients_.at(0); +} + +std::vector Regression::GetCoeffs(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); + + std::vector coefficients{beta1, beta0}; + return coefficients; +} +} // namespace hyped::utils::math \ No newline at end of file From 2dea38db5028efb1a9c3c24a36a3bbbca5c4dadb Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Tue, 29 Mar 2022 18:02:26 +0100 Subject: [PATCH 2/6] Structure edit --- src/propulsion/main.cpp | 4 +++- src/propulsion/rpm_regulator.cpp | 4 +++- src/utils/math/regression.hpp | 18 ++++++------------ 3 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/propulsion/main.cpp b/src/propulsion/main.cpp index 008d3396..7edfefe8 100644 --- a/src/propulsion/main.cpp +++ b/src/propulsion/main.cpp @@ -55,7 +55,9 @@ void Main::run() break; case data::State::kCalibrating: if (state_processor_.isInitialised()) { - hyped::utils::math::Regression::Regression(); + // TODO: aruments read in from somewhere and processed + utils::math::Regression regression; + regression.GetCoeffs(std::vector{1, 1, 1}, std::vector{2, 2, 2}); 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 cf33d4d0..33374699 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() @@ -18,7 +20,7 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity) { - struct hyped::utils::math::Regression regression; + hyped::utils::math::Regression regression; const double beta1 = regression.Coefficients.beta1; const double beta0 = regression.Coefficients.beta0; diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp index 15325e87..1fd9f28b 100644 --- a/src/utils/math/regression.hpp +++ b/src/utils/math/regression.hpp @@ -8,26 +8,19 @@ namespace hyped::utils::math { class Regression { public: Regression(); - std::vector GetCoeffs(std::vector x_data, std::vector y_data); + void GetCoeffs(std::vector x_data, std::vector y_data); struct Coefficients { const double beta0; const double beta1; - } coefficients; + }; + Coefficients coefficients; private: std::vector coefficients_; }; -Regression::Regression() -{ - coefficients_ = GetCoeffs(std::vector x_data, std::vector y_data); - coefficients.beta0 = Regression::coefficients_.at(1); - coefficients.beta1 = Regression::coefficients_.at(0); -} - -std::vector Regression::GetCoeffs(const std::vector x_data, - const std::vector y_data) +void Regression::GetCoeffs(const std::vector x_data, const std::vector y_data) { double x_sum = 0; double y_sum = 0; @@ -53,6 +46,7 @@ std::vector Regression::GetCoeffs(const std::vector x_data, double beta0 = y_mean - (beta1 - x_mean); std::vector coefficients{beta1, beta0}; - return coefficients; + Coefficients.beta0 = coefficients.at(1); + Coefficients.beta1 = coefficients.at(0); } } // namespace hyped::utils::math \ No newline at end of file From ba46beed31c669f8e780f900d6c7fe83227f2844 Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Tue, 29 Mar 2022 18:17:51 +0100 Subject: [PATCH 3/6] structure update --- src/utils/math/regression.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp index 1fd9f28b..65dd5e0d 100644 --- a/src/utils/math/regression.hpp +++ b/src/utils/math/regression.hpp @@ -45,8 +45,8 @@ void Regression::GetCoeffs(const std::vector x_data, const std::vector coefficients{beta1, beta0}; - Coefficients.beta0 = coefficients.at(1); - Coefficients.beta1 = coefficients.at(0); + std::vector coeffs{beta1, beta0}; + coefficients.beta0 = coeffs.at(1); + coefficients.beta1 = coeffs.at(0); } } // namespace hyped::utils::math \ No newline at end of file From 1279797b9c96cf387138d2bee11f9cbc73a00693 Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Thu, 31 Mar 2022 18:51:56 +0100 Subject: [PATCH 4/6] make code less broken --- src/propulsion/rpm_regulator.cpp | 4 ++-- src/utils/math/regression.hpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/propulsion/rpm_regulator.cpp b/src/propulsion/rpm_regulator.cpp index 33374699..9c767db4 100644 --- a/src/propulsion/rpm_regulator.cpp +++ b/src/propulsion/rpm_regulator.cpp @@ -21,8 +21,8 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity) { hyped::utils::math::Regression regression; - const double beta1 = regression.Coefficients.beta1; - const double beta0 = regression.Coefficients.beta0; + const double beta1 = regression.coefficients.beta1; + const double beta0 = regression.coefficients.beta0; // polynomial values from simulation return std::round(beta1 * actual_velocity + beta0); diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp index 65dd5e0d..0e4472f6 100644 --- a/src/utils/math/regression.hpp +++ b/src/utils/math/regression.hpp @@ -11,8 +11,8 @@ class Regression { void GetCoeffs(std::vector x_data, std::vector y_data); struct Coefficients { - const double beta0; - const double beta1; + double beta0; + double beta1; }; Coefficients coefficients; From 0649fd62068c695f7bfacec0e2eb19a7bf81dac5 Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Mon, 16 May 2022 18:36:44 +0100 Subject: [PATCH 5/6] changed class setup. comment out function calls until class properly defined --- src/propulsion/main.cpp | 3 --- src/propulsion/rpm_regulator.cpp | 4 ++++ src/utils/math/regression.cpp | 34 ++++++++++++++++++++++++++++++++ src/utils/math/regression.hpp | 30 ---------------------------- 4 files changed, 38 insertions(+), 33 deletions(-) create mode 100644 src/utils/math/regression.cpp diff --git a/src/propulsion/main.cpp b/src/propulsion/main.cpp index 7edfefe8..a56259db 100644 --- a/src/propulsion/main.cpp +++ b/src/propulsion/main.cpp @@ -55,9 +55,6 @@ void Main::run() break; case data::State::kCalibrating: if (state_processor_.isInitialised()) { - // TODO: aruments read in from somewhere and processed - utils::math::Regression regression; - regression.GetCoeffs(std::vector{1, 1, 1}, std::vector{2, 2, 2}); 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 9c767db4..d172f555 100644 --- a/src/propulsion/rpm_regulator.cpp +++ b/src/propulsion/rpm_regulator.cpp @@ -20,12 +20,16 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity) { + // TODO: get regression coeffs from class! + /* hyped::utils::math::Regression regression; const double beta1 = regression.coefficients.beta1; const double beta0 = regression.coefficients.beta0; // polynomial values from simulation 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/utils/math/regression.cpp b/src/utils/math/regression.cpp new file mode 100644 index 00000000..e1323a19 --- /dev/null +++ b/src/utils/math/regression.cpp @@ -0,0 +1,34 @@ +#include "regression.hpp" + +namespace hyped::utils::math { + +void Regression::GetCoeffs(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); + + std::vector coeffs{beta1, beta0}; + coefficients.beta0 = coeffs.at(1); + coefficients.beta1 = coeffs.at(0); +} +} // namespace hyped::utils::math \ No newline at end of file diff --git a/src/utils/math/regression.hpp b/src/utils/math/regression.hpp index 0e4472f6..12e65c40 100644 --- a/src/utils/math/regression.hpp +++ b/src/utils/math/regression.hpp @@ -19,34 +19,4 @@ class Regression { private: std::vector coefficients_; }; - -void Regression::GetCoeffs(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); - - std::vector coeffs{beta1, beta0}; - coefficients.beta0 = coeffs.at(1); - coefficients.beta1 = coeffs.at(0); -} } // namespace hyped::utils::math \ No newline at end of file From 56f86c16e8adf9b269617c95232d1435e577a202 Mon Sep 17 00:00:00 2001 From: maxguy2001 Date: Sun, 22 May 2022 05:44:32 +0100 Subject: [PATCH 6/6] implemented linear regression class --- src/propulsion/main.cpp | 8 ++++++++ src/propulsion/rpm_regulator.cpp | 13 +++++++------ src/propulsion/rpm_regulator.hpp | 11 +++++++++++ src/utils/math/regression.cpp | 11 +++++++---- src/utils/math/regression.hpp | 16 +++++++++------- 5 files changed, 42 insertions(+), 17 deletions(-) diff --git a/src/propulsion/main.cpp b/src/propulsion/main.cpp index a56259db..8f44ee6a 100644 --- a/src/propulsion/main.cpp +++ b/src/propulsion/main.cpp @@ -55,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 d172f555..116edbc4 100644 --- a/src/propulsion/rpm_regulator.cpp +++ b/src/propulsion/rpm_regulator.cpp @@ -18,17 +18,18 @@ 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) { - // TODO: get regression coeffs from class! - /* - hyped::utils::math::Regression regression; - const double beta1 = regression.coefficients.beta1; - const double beta0 = regression.coefficients.beta0; + double beta1 = coefficients_.at(0); + double beta0 = coefficients_.at(1); // polynomial values from simulation return std::round(beta1 * actual_velocity + beta0); - */ return 0; } diff --git a/src/propulsion/rpm_regulator.hpp b/src/propulsion/rpm_regulator.hpp index 1db0f075..ac06c64e 100644 --- a/src/propulsion/rpm_regulator.hpp +++ b/src/propulsion/rpm_regulator.hpp @@ -20,6 +20,14 @@ class RpmRegulator { */ 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 calculates the optimal rpm based off of the current velocity. @@ -37,6 +45,9 @@ class RpmRegulator { * @return int32_t - the step with which to increase the 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/math/regression.cpp b/src/utils/math/regression.cpp index e1323a19..49de3f11 100644 --- a/src/utils/math/regression.cpp +++ b/src/utils/math/regression.cpp @@ -2,7 +2,12 @@ namespace hyped::utils::math { -void Regression::GetCoeffs(const std::vector x_data, const std::vector y_data) +Regression::Regression() +{ +} + +void Regression::CaulculateCoefficients(const std::vector x_data, + const std::vector y_data) { double x_sum = 0; double y_sum = 0; @@ -27,8 +32,6 @@ void Regression::GetCoeffs(const std::vector x_data, const std::vector coeffs{beta1, beta0}; - coefficients.beta0 = coeffs.at(1); - coefficients.beta1 = coeffs.at(0); + 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 index 12e65c40..3828dfbe 100644 --- a/src/utils/math/regression.hpp +++ b/src/utils/math/regression.hpp @@ -8,15 +8,17 @@ namespace hyped::utils::math { class Regression { public: Regression(); - void GetCoeffs(std::vector x_data, std::vector y_data); - struct Coefficients { - double beta0; - double beta1; - }; - Coefficients coefficients; + /** + * @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); - private: + // calculated coefficients from regression function std::vector coefficients_; }; } // namespace hyped::utils::math \ No newline at end of file