Editing PIDtest
[fresonbot.git] / PID_test.py
index 03cd752..78f3a42 100644 (file)
@@ -39,7 +39,9 @@ def changeHeading():
 
     print ('referenceHeading {0:.4f}'.format(referenceHeading))
 
-    headingError = referenceHeading - robot1.heading
+#    headingError = referenceHeading - robot1.heading
+    headingError = (referenceHeading - robot1.heading+ math.pi)%(2*math.pi) - math.pi
+
     print ('headingError {0:.4f}'.format(headingError))
     print ('deltax {0:.4f}'.format(deltax))
     print ('deltay {0:.4f}'.format(deltay))