diff --git a/esp32/lib/ESP32-sveltekit/MathUtils.h b/esp32/lib/ESP32-sveltekit/MathUtils.h
new file mode 100644
index 0000000..9c89525
--- /dev/null
+++ b/esp32/lib/ESP32-sveltekit/MathUtils.h
@@ -0,0 +1,12 @@
+#ifndef MATHUTILS_H
+#define MATHUTILS_H
+
+#include <cmath>
+
+inline float lerp(float start, float end, float t) { return (1 - t) * start + t * end; }
+
+inline bool isEqual(float a, float b, float epsilon) { return std::fabs(a - b) < epsilon; }
+
+inline float round2(float value) { return (int)(value * 100 + 0.5) / 100.0; }
+
+#endif
\ No newline at end of file
diff --git a/esp32/lib/ESP32-sveltekit/MotionService.h b/esp32/lib/ESP32-sveltekit/MotionService.h
index 9614b2a..ae7a4f1 100644
--- a/esp32/lib/ESP32-sveltekit/MotionService.h
+++ b/esp32/lib/ESP32-sveltekit/MotionService.h
@@ -4,6 +4,7 @@
 #include <EventSocket.h>
 #include <TaskManager.h>
 #include <Kinematics.h>
+#include <MathUtils.h>
 
 #define DEFAULT_STATE false
 #define LIGHT_SETTINGS_ENDPOINT_PATH "/api/input"
@@ -91,8 +92,6 @@ class MotionService {
         _socket->emit(ANGLES_EVENT, output, originId.c_str());
     }
 
-    float lerp(float start, float end, float t) { return (1 - t) * start + t * end; }
-
     bool updateMotion() {
         switch (motionState) {
             case MOTION_STATE::IDLE: return false; break;
diff --git a/esp32/lib/ESP32-sveltekit/Peripherals.h b/esp32/lib/ESP32-sveltekit/Peripherals.h
index 17f5dab..95fccfc 100644
--- a/esp32/lib/ESP32-sveltekit/Peripherals.h
+++ b/esp32/lib/ESP32-sveltekit/Peripherals.h
@@ -6,6 +6,7 @@
 #include <HttpEndpoint.h>
 #include <SecurityManager.h>
 #include <StatefulService.h>
+#include <MathUtils.h>
 #include <list>
 
 #include <SPI.h>
@@ -333,8 +334,6 @@ class Peripherals : public StatefulService<PeripheralsConfiguration> {
         return temperature;
     }
 
-    double round2(double value) { return (int)(value * 100 + 0.5) / 100.0; }
-
   protected:
     void updateImu() {
         doc.clear();