Michael Shimniok / Mbed 2 deprecated DataBus

Code for autonomous rover for Sparkfun AVC. DataBus won 3rd in 2012 and the same code was used on Troubled Child, a 1986 Jeep Grand Wagoneer to win 1st in 2014.

Dependencies:   mbed Watchdog SDFileSystem DigoleSerialDisp

Diff: util.cpp

Revision:
3:42f3821c4e54
Parent:
2:fbc6e3cf3ed8
Child:
18:c2f3df4ef5fe
--- a/util.cpp	Thu Jun 06 13:40:23 2013 +0000
+++ b/util.cpp	Fri Jun 07 14:45:46 2013 +0000
@@ -1,12 +1,24 @@
+#include <math.h>
+
 /** 
  * Clamp a value (angle) between min (non-inclusive) and max (inclusive)
  * e.g. clamp(v, 0, 360) or clamp(v, -180, 180)
  */
-float clamp(float v, float min, float max) 
+float clamp(float v, float min, float max, bool flip) 
 {
+    float i;
+    float f;
     float mod = (max - min);
-    if (v >= max) float -= mod;
-    if (v < min) float += mod;
+
+    f = modff((v/mod), &i) * mod;
+    if (flip) {
+        if (f > max) f -= mod;
+        if (f <= min) f += mod;
+    } else {
+        if (f < min) f += mod;
+        if (f >= max) f -= mod;
+    }
+    return f;
 }
 
 // convert character to an int