Editing PIDtest
authorJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 17:18:47 +0000 (17:18 +0000)
committerJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 17:18:47 +0000 (17:18 +0000)
PID_test.py

index cfb1aeb..7d5a139 100644 (file)
@@ -8,23 +8,19 @@ import subprocess
 import pinout # pinout.py is where I defined all used pins in BBB
 from Fresonbot import Fresonbot
 
-subprocess.call("bashScripts/enable_encoder_slots.sh") # Enabling encoders
-
-GPIO.setup(pinout.PinMotorLeftPhase, GPIO.OUT)
-GPIO.setup(pinout.PinMotorRightPhase, GPIO.OUT)
-
-fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
-fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
-
-TicksxRevolution = 1200 # Depending on the encoder
-
-## you have to take this measures accurately
-WheelRadius = 31.8/2 # I took the diameter and divided by 2
-WheelDistance = 88.9 # between centers
-
 #global distance
 robot1 = Fresonbot() # Using Fresonbot class
 
+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([100,0])
+
+# PID constants
+Kp = 150
+Ki = 0.1
+Kd = 0.001
+
 def changeHeading():
     global u
     global headingError
@@ -90,44 +86,29 @@ def goTarget():
 
 IncTicksLeft = 0
 IncTicksRight = 0
-#x = 0
-#y = 0
-#heading = 0
+
 distanceLeft = 0
 distanceRight = 0
-#distance = 0
+
 lastHeadingError = 0
 headingErrorIntegral = 0
 velocityTranslational = 50
 sensitivityLimit = 5
 u = 0
 
-# PID constants
-Kp = 150
-Ki = 0.1
-Kd = 0.001
-
 VelLeft = 50
 VelRight = 50
 
 robot1.getTicks();
 
-#StartTicksLeft = robot1.TicksLeft 
-#StartTicksRight = robot1.TicksRight 
-
 start_dt = time.time() * 1000
 
-#motion();
 robot1.motion(velocityTranslational,velocityTranslational)
 
 time.sleep(0.02)
 
 robot1.getPosition();
 
-#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([100,0])
 for xT, yT in zip(it, it):
     xTarget = xT
     yTarget = yT