-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[Math] Separate out util math from regular math
Topic: math-splitup Relative: Reviewers:
- Loading branch information
1 parent
2ca2a89
commit c09feb7
Showing
9 changed files
with
137 additions
and
117 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -2,6 +2,7 @@ | |
|
||
#include "math.h" | ||
#include "math_foc.hpp" | ||
#include "math_util.hpp" | ||
|
||
namespace control_loop { | ||
|
||
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,61 @@ | ||
#include "math_util.hpp" | ||
|
||
#include "gtest/gtest.h" | ||
#include "math.h" | ||
|
||
namespace math { | ||
|
||
// Test the wraparound function | ||
TEST(MathUtilTest, test_wraparound) { | ||
float angle = -0.4f; | ||
|
||
// Wrap the angle around 0 -> 1. Expect the result to be 0.6 | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_FLOAT_EQ(angle, 0.6f); | ||
|
||
// Make the angle 1.2. Expect the result to be 0.2 | ||
angle = 1.2f; | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_FLOAT_EQ(angle, 0.2f); | ||
|
||
// Make the angle 0.5. Expect the result to be 0.5 | ||
angle = 0.5f; | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_FLOAT_EQ(angle, 0.5f); | ||
|
||
// Make the angle 0.0. Expect the result to be 0.0 | ||
angle = 0.0f; | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_FLOAT_EQ(angle, 0.0f); | ||
|
||
// Make the angle -46.0. Expect the result to be 0.0 | ||
angle = -46.0f; | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_FLOAT_EQ(angle, 0.0f); | ||
|
||
// Make the angle 45.3. Expect the result to be 0.3 | ||
angle = 45.3001f; | ||
math::wraparound(angle, 0.0f, 1.0f); | ||
EXPECT_NEAR(angle, 0.3001f, 0.01); | ||
} | ||
|
||
// Test the trapezoidal integral function | ||
TEST(MathUtilTest, test_trapezoidal_integral) { | ||
float x = 1.0f; | ||
float x_prev = 0.0f; | ||
float y = 1.0f; | ||
float y_prev = 0.0f; | ||
|
||
float result = math::trapezoidal_integral(x, x_prev, y, y_prev); | ||
EXPECT_FLOAT_EQ(result, 0.5f); | ||
|
||
// Test it now with x = 2.0f, x_prev = 1.0f, y = -1.0f, y_prev = 1.0f | ||
x = 2.0f; | ||
x_prev = 1.0f; | ||
y = -1.0f; | ||
y_prev = 1.0f; | ||
result = math::trapezoidal_integral(x, x_prev, y, y_prev); | ||
EXPECT_FLOAT_EQ(result, 0.0f); | ||
} | ||
|
||
} // namespace math |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,13 @@ | ||
#include "math_util.hpp" | ||
|
||
namespace math { | ||
float low_pass_filter(float input, float prev_output, float tau, float dt) { | ||
float alpha = dt / (tau + dt); | ||
return alpha * input + (1.0f - alpha) * prev_output; | ||
} | ||
|
||
float trapezoidal_integral(float x, float x_prev, float y, float y_prev) { | ||
float integral = (x - x_prev) * (y + y_prev) / 2.0f; | ||
return integral; | ||
} | ||
} // namespace math |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,59 @@ | ||
#ifndef MATH_UTIL_HPP | ||
#define MATH_UTIL_HPP | ||
|
||
namespace math { | ||
/** | ||
* @brief Integrate the given value using the trapezoidal rule. | ||
* @param x The current x value | ||
* @param x_prev The previous x value | ||
* @param y The current y value | ||
* @param y_prev The previous y value | ||
*/ | ||
float trapezoidal_integral(float x, float x_prev, float y, float y_prev); | ||
|
||
/** | ||
* @brief Clamp the given value between the given min and max values. | ||
* @param value The value to clamp | ||
* @param min The minimum value | ||
* @param max The maximum value | ||
* @return void | ||
*/ | ||
template <typename T> | ||
void clamp(T& value, const T& min, const T& max) { | ||
if (value < min) { | ||
value = min; | ||
} else if (value > max) { | ||
value = max; | ||
} else { | ||
// do nothing | ||
} | ||
} | ||
|
||
/** | ||
* @brief Wrap the given value around the given min and max values. | ||
* @param value The value to wrap | ||
* @param min The minimum value | ||
* @param max The maximum value | ||
* @return void | ||
*/ | ||
template <typename T> | ||
void wraparound(T& value, const T& min, const T& max) { | ||
while (value < min) { | ||
value += (max - min); | ||
} | ||
while (value > max) { | ||
value -= (max - min); | ||
} | ||
} | ||
|
||
/** | ||
* @brief Low pass filter the given input value. | ||
* @param input The input value | ||
* @param prev_output The previous output value | ||
* @param tau The time constant of the filter | ||
* @param dt The time step | ||
*/ | ||
float low_pass_filter(float input, float prev_output, float tau, float dt); | ||
} // namespace math | ||
|
||
#endif // MATH_UTIL_HPP |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,6 +1,6 @@ | ||
#include "pid.hpp" | ||
|
||
#include "math_foc.hpp" | ||
#include "math_util.hpp" | ||
|
||
namespace pid { | ||
|
||
|