updated control.unwrap to unwrap phase changes greater than 2pi

Sawyer Fuller · Sawyer Fuller · commit 78ca1e992421 · 2011-03-31T16:43:19.000Z
diff --git a/src/ctrlutil.py b/src/ctrlutil.py
@@ -71,9 +71,9 @@ def unwrap(angle, period=2*pi):
     for k in range(len(angle)):
         # See if we need to account for angle wrapping
         if (angle[k] - last > period/2):
-            wrap = -period
+            wrap -= period
         elif (last - angle[k] > period/2):
-            wrap = period
+            wrap += period
 
         # Update the last value we have sene
         last = angle[k]
