From 5de8b7cb19a8407dc8677cff2dfb8ae5f6ae1c46 Mon Sep 17 00:00:00 2001
From: philsson <philsson@gmail.com>
Date: Sun, 2 Sep 2018 19:02:49 +0200
Subject: [PATCH] Constrain range helper function

---
 Makefile               |  2 ++
 src/math/Utilities.cpp | 20 ++++++++++++++++++++
 src/math/Utilities.h   | 10 ++++++++++
 3 files changed, 32 insertions(+)
 create mode 100644 src/math/Utilities.cpp
 create mode 100644 src/math/Utilities.h

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