Editing PIDtest
authorJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 19:01:35 +0000 (19:01 +0000)
committerJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 19:01:35 +0000 (19:01 +0000)
PID_test.py

index d31bdec..0e61c87 100644 (file)
@@ -34,14 +34,19 @@ def changeHeading():
     deltax = xTarget - robot1.x
     deltay = yTarget - robot1.y
 
     deltax = xTarget - robot1.x
     deltay = yTarget - robot1.y
 
-    if ((deltay >= 0) and (deltax >= 0)):
-        referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
-    if ((deltay < 0) and (deltax < 0)):
-        referenceHeading = (math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
-    if ((deltay >= 0) and (deltax < 0)):
-        referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
-    if ((deltay < 0) and (deltax >= 0)):
-        referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
+#    if ((deltay >= 0) and (deltax >= 0)):
+#        referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
+#    if ((deltay < 0) and (deltax < 0)):
+#        referenceHeading = (math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
+#    if ((deltay >= 0) and (deltax < 0)):
+#        referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
+#    if ((deltay < 0) and (deltax >= 0)):
+#        referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
+
+#    referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
+
+    referenceHeading =  math.atan2(deltay, deltax)
+ #   referenceHeading = (referenceHeading + math.pi)/(2*math.pi)-math.pi
 
     print ('referenceHeading {0:.4f}'.format(referenceHeading))
 
 
     print ('referenceHeading {0:.4f}'.format(referenceHeading))