Editing PIDtest
authorJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 17:06:22 +0000 (17:06 +0000)
committerJordi <jordiguerrero@gmail.com>
Sat, 13 Feb 2016 17:06:22 +0000 (17:06 +0000)
Fresonbot.py
Fresonbot.pyc
PID_test.py
pinout.py
pinout.pyc

index e4e9fad..c259d84 100644 (file)
@@ -10,6 +10,9 @@ import pinout
 class Fresonbot(object):
 
     ticksPerTurn = 12 *100 # 12 counts per revolution * 100:1 reduction gearbox
+## you have to take this measures accurately
+#    WheelRadius = 31.8/2 # I took the diameter and divided by 2
+#    WheelDistance = 88.9 # between centers
     WheelRadius = 16 # In mm
     WheelDistance = 89 # In mm
 
index 233825e..78e0325 100644 (file)
Binary files a/Fresonbot.pyc and b/Fresonbot.pyc differ
index a167e92..cfb1aeb 100644 (file)
-###
-# arg1 xTarget
-# arg2 yTarget
-
 import Adafruit_BBIO.PWM as PWM
 import Adafruit_BBIO.GPIO as GPIO
 
 import time
-import sys
 import math
 import subprocess
 
-PinMotorLeftPwm = "P9_14"
-PinMotorRightPwm = "P9_16"
-PinMotorLeftPhase = "P9_12"
-PinMotorRightPhase = "P9_23"
+import pinout # pinout.py is where I defined all used pins in BBB
+from Fresonbot import Fresonbot
 
-subprocess.call("bashScripts/enable_encoder_slots.sh")
+subprocess.call("bashScripts/enable_encoder_slots.sh") # Enabling encoders
 
-GPIO.setup(PinMotorLeftPhase, GPIO.OUT)
-GPIO.setup(PinMotorRightPhase, GPIO.OUT)
+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
-#WheelRadius = 16
-WheelRadius = 31.8/2
-#WheelDistance = 85.5
-#WheelDistance = 89
-WheelDistance = 88.9
-
-global distance
-
-def motion():
-    AbsVelLeft = abs(VelLeft)
-    AbsVelRight = abs(VelRight)
-    if (VelLeft < 0):
-        PhaseLeft = 1
-    else:
-        PhaseLeft = 0
-    if (VelRight < 0):
-        PhaseRight = 1
-    else:
-        PhaseRight = 0
-    if (AbsVelLeft > 100):
-        AbsVelLeft = 100
-    if (AbsVelRight > 100):
-        AbsVelRight = 100
-
-    GPIO.output(PinMotorLeftPhase, PhaseLeft)
-    GPIO.output(PinMotorRightPhase, PhaseRight)
-    PWM.set_duty_cycle(PinMotorLeftPwm,AbsVelLeft)
-    PWM.set_duty_cycle(PinMotorRightPwm,AbsVelRight)
-    return 
-
-def getTicks():
-    global TicksLeft
-    global TicksRight
-    foTicksLeft = open(fTicksLeft, "r")
-    foTicksRight = open(fTicksRight, "r")
-
-    TicksLeft = foTicksLeft.read()
-    TicksLeft = int(TicksLeft.split('\n', 1)[0])
-    TicksRight = foTicksRight.read()
-    TicksRight = int(TicksRight.split('\n', 1)[0])
-
-    foTicksLeft.close()
-    foTicksRight.close()
-    return
-
-def stop():
-    GPIO.output(PinMotorLeftPhase,0)
-    GPIO.output(PinMotorRightPhase,0)
-    PWM.start(PinMotorLeftPwm,0)
-    PWM.start(PinMotorRightPwm,0)
-    GPIO.cleanup()
-    return
-
-def measureDistance():
-    global distance
-    global heading
-    global x
-    global y
-    global StartTicksLeft 
-    global StartTicksRight
-
-    getTicks();
-    EndTicksLeft = TicksLeft
-    EndTicksRight = TicksRight
-
-    IncTicksLeft = EndTicksLeft - StartTicksLeft
-    IncTicksRight = EndTicksRight - StartTicksRight
+TicksxRevolution = 1200 # Depending on the encoder
 
-    distanceLeft = 2 * math.pi * WheelRadius * (float(IncTicksLeft) / TicksxRevolution)
-    distanceRight = 2 * math.pi * WheelRadius * (float(IncTicksRight) / TicksxRevolution)
+## you have to take this measures accurately
+WheelRadius = 31.8/2 # I took the diameter and divided by 2
+WheelDistance = 88.9 # between centers
 
-    newdistance = (distanceLeft + distanceRight) / 2
-    distance += newdistance
-
-    heading += (distanceLeft - distanceRight) / WheelDistance
-    x += newdistance * math.cos(heading)
-    y += newdistance * math.sin(heading)
-    print ('heading {0:.4f}'.format(heading))
-    print ('x {0:.4f}'.format(x))
-    print ('y {0:.4f}'.format(y))
-
-    StartTicksLeft = EndTicksLeft
-    StartTicksRight = EndTicksRight
-
-    return
+#global distance
+robot1 = Fresonbot() # Using Fresonbot class
 
 def changeHeading():
     global u
@@ -119,8 +34,8 @@ def changeHeading():
     global VelLeft
     global VelRight
 
-    deltax = xTarget - x
-    deltay = yTarget - 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)))
@@ -133,7 +48,7 @@ def changeHeading():
 
     print ('referenceHeading {0:.4f}'.format(referenceHeading))
 
-    headingError = referenceHeading - heading
+    headingError = referenceHeading - robot1.heading
     print ('headingError {0:.4f}'.format(headingError))
     print ('deltax {0:.4f}'.format(deltax))
     print ('deltay {0:.4f}'.format(deltay))
@@ -144,8 +59,6 @@ def changeHeading():
 
     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
 
-#    VelLeft = (2*velocityTranslational + u*WheelDistance) / (2*WheelRadius)
-#    VelRight = (2*velocityTranslational - u*WheelDistance) / (2*WheelRadius)
     VelLeft = (velocityTranslational + u)
     VelRight = (velocityTranslational - u)
 
@@ -157,7 +70,7 @@ def goTarget():
     global xTarget
     global yTarget
 
-    while ((x > xTarget + sensitivityLimit) or (x < xTarget - sensitivityLimit)) or ((y > yTarget + sensitivityLimit) or (y < yTarget - sensitivityLimit)):
+    while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
 
         end_dt = time.time() * 1000
         dt = end_dt - start_dt
@@ -166,69 +79,64 @@ def goTarget():
         print ('***********************')
         start_dt = end_dt
 
-        measureDistance();
+        robot1.getPosition();
 
         changeHeading();
 
-        motion();
+        robot1.motion(VelLeft, VelRight)
 
         time.sleep(0.02)
     return
 
 IncTicksLeft = 0
 IncTicksRight = 0
-x = 0
-y = 0
-heading = 0
+#x = 0
+#y = 0
+#heading = 0
 distanceLeft = 0
 distanceRight = 0
-distance = 0
+#distance = 0
 lastHeadingError = 0
 headingErrorIntegral = 0
 velocityTranslational = 50
 sensitivityLimit = 5
 u = 0
-Kp = 150
 
+# PID constants
+Kp = 150
 Ki = 0.1
-
 Kd = 0.001
 
 VelLeft = 50
 VelRight = 50
-#xTarget = int(sys.argv[1])
-#yTarget = int(sys.argv[2])
 
-getTicks();
+robot1.getTicks();
 
-StartTicksLeft = TicksLeft 
-StartTicksRight = TicksRight 
+#StartTicksLeft = robot1.TicksLeft 
+#StartTicksRight = robot1.TicksRight 
 
 start_dt = time.time() * 1000
 
 #motion();
-GPIO.output(PinMotorLeftPhase, 0)
-GPIO.output(PinMotorRightPhase, 0)
-PWM.start(PinMotorLeftPwm,50)
-PWM.start(PinMotorRightPwm,50)
+robot1.motion(velocityTranslational,velocityTranslational)
 
 time.sleep(0.02)
 
-measureDistance();
+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,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,50])
+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
     goTarget();
 
-stop();
+robot1.stop();
 
 time.sleep(0.5)
 
-getTicks();
+robot1.getTicks();
 
-measureDistance();
+robot1.getPosition();
index 449c785..76eea69 100644 (file)
--- a/pinout.py
+++ b/pinout.py
@@ -4,16 +4,16 @@ PinMotorLeftPhase = "P9_12"
 PinMotorRightPhase = "P9_23"
 
 #     
-#     _1_
-#  3/     \0
+#     _2_
+#  3/     \1
 #  |       |
 #  |       |
-# 4|       |2
+# 4|       |0
 #
 
 
-#PinIRRightB = "AIN2"
-#PinIRRightF = "AIN0"
-#PinIRFront = "AIN1"
-#PinIRLeftF = "AIN3"
-#PinIRLeftB = "AIN4"
+PinIRRightB = "AIN0"
+PinIRRightF = "AIN1"
+PinIRFront = "AIN2"
+PinIRLeftF = "AIN3"
+PinIRLeftB = "AIN4"
index 54b2942..9726618 100644 (file)
Binary files a/pinout.pyc and b/pinout.pyc differ