Diff: utilityFunctions.h

Revision:

7:a8c2e9d049e8

diff -r f64b1eba4d5e -r a8c2e9d049e8 utilityFunctions.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/utilityFunctions.h	Fri Dec 16 14:06:42 2016 +0000
@@ -0,0 +1,41 @@
+#ifndef UTIL_FUNC_H
+#define UTIL_FUNC_H
+
+#define Pi  (3.14159)
+#define DEG2RAD (Pi/180)
+#define RAD2DEG (180/Pi)
+float saturateCmd(float cmd, float max,float min)
+{
+    if(cmd>max) {
+        cmd = max;
+    }
+    if(cmd < min) {
+        cmd = min;
+    }
+    return cmd;
+}
+float saturateCmd(float cmd)
+{
+    if(cmd>1.0) {
+        cmd = 1.0;
+    }
+    if(cmd < -1.0) {
+        cmd = -1.0;
+    }
+    return cmd;
+}
+float wrapToPi(float ang)
+{
+
+    while(ang > Pi) {
+
+        ang = ang - 2*Pi;
+    }
+    while(ang < -Pi) {
+        ang = ang + 2*Pi;
+    }
+
+    return ang;
+}
+
+#endif  
\ No newline at end of file