Cooper Liu / Mbed 2 deprecated Eurobot2013_Co-Processor

working version with calibration done

Fork of Eurobot2013 by Oskar Weigl

Diff: geometryfuncs/geometryfuncs.h

Revision:
10:2bd9f4e02b74
Parent:
6:5a52c046d8f7
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/geometryfuncs/geometryfuncs.h	Sun Apr 07 16:30:49 2013 +0000
@@ -0,0 +1,18 @@
+#ifndef GEOMETRYFUNCS_H
+#define GEOMETRYFUNCS_H
+
+// rectifies angle to range -PI to PI
+template <typename T>
+T rectifyAng (T ang_in) {
+    ang_in -= (floor(ang_in/(2*PI)))*2*PI;
+    if (ang_in < -PI) {
+        ang_in += 2*PI;
+    }
+    if (ang_in > PI) {
+        ang_in -= 2*PI;
+    }
+
+    return ang_in;
+}
+
+#endif //GEOMETRYFUNCS_H
\ No newline at end of file