diff --git a/Makefile b/Makefile
index 4a4858f..1ce3ac6 100644
--- a/Makefile
+++ b/Makefile
@@ -645,6 +645,8 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/math/Utilities.o
+
 
 
 INCLUDE_PATHS += -I../
diff --git a/src/math/Utilities.cpp b/src/math/Utilities.cpp
new file mode 100644
index 0000000..acf8f9c
--- /dev/null
+++ b/src/math/Utilities.cpp
@@ -0,0 +1,20 @@
+#include "src/math/utilities.h"
+
+#include <stdlib.h>
+
+namespace math {
+
+float constrain(float value, float range)
+{
+    if (value < -range)
+    {
+        return -range;
+    }
+    if (value > range)
+    {
+        return range;
+    }
+    return value;
+}
+    
+}
\ No newline at end of file
diff --git a/src/math/Utilities.h b/src/math/Utilities.h
new file mode 100644
index 0000000..d49558d
--- /dev/null
+++ b/src/math/Utilities.h
@@ -0,0 +1,10 @@
+#ifndef PI
+  #define PI 3.14159265358979f
+#endif
+
+
+namespace math {
+
+float constrain(float value, float range);
+
+}
\ No newline at end of file