Constrain range helper function

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../

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;
+}
+    
+}

src/math/Utilities.h

@@ -0,0 +1,10 @@
+#ifndef PI
+  #define PI 3.14159265358979f
+#endif
+
+
+namespace math {
+
+float constrain(float value, float range);
+
+}