Editing PIDtest
authorJordi <jordiguerrero@gmail.com>
Sun, 14 Feb 2016 11:55:07 +0000 (11:55 +0000)
committerJordi <jordiguerrero@gmail.com>
Sun, 14 Feb 2016 11:55:07 +0000 (11:55 +0000)
PID_test.py

index f0544c8..e4ececd 100644 (file)
@@ -8,22 +8,21 @@ import subprocess
 import pinout # pinout.py is where I defined all used pins in BBB
 from Fresonbot import Fresonbot
 
-#global distance
 robot1 = Fresonbot() # Using Fresonbot class
 
 # Use it variable to describe your trajectory ([x1,y2,x2,y2,....,xn,yn])
-it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
+#it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
 #it = iter([400,0,400,400,0,400,200,200,0,0])
-#it = iter([200,0,200,200,0,200,0,0])
+it = iter([200,0,200,200,0,200,0,0])
 #it = iter([100,0])
 
 # PID constants
 #Kp = 150
 Kp = 200
-#Ki = 0.1
 Ki = 0.1
-#Kd = 0.001
 Kd = 0.001
+#Change velocityTranslational to increase or decrease velocity
+velocityTranslational = 70 
 
 def changeHeading():
     global u
@@ -38,17 +37,9 @@ def changeHeading():
     deltay = yTarget - robot1.y
 
     referenceHeading =  math.atan2(deltay, deltax)
- #   referenceHeading = (referenceHeading + math.pi)/(2*math.pi)-math.pi
-
-    print ('referenceHeading {0:.4f}'.format(referenceHeading))
-
 #    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))
-
     headingErrorDerivative = (headingError - lastHeadingError) / dt
     headingErrorIntegral += (headingError * dt)
     lastHeadingError = headingError
@@ -70,9 +61,6 @@ def goTarget():
 
         end_dt = time.time() * 1000
         dt = end_dt - start_dt
-    
-        print ('dt {0:.4f}'.format(dt))
-        print ('***********************')
         start_dt = end_dt
 
         robot1.getPosition();
@@ -91,11 +79,10 @@ distanceLeft = 0
 distanceRight = 0
 lastHeadingError = 0
 headingErrorIntegral = 0
-velocityTranslational = 70
 sensitivityLimit = 5
 u = 0
-VelLeft = 50
-VelRight = 50
+VelLeft = velocityTranslational
+VelRight = velocityTranslational
 
 #Main program
 robot1.getTicks();