Editing PIDtest
authorJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 19:12:12 +0000 (19:12 +0000)
committerJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 19:12:12 +0000 (19:12 +0000)
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))