PID OK
authorJordi <jordiguerrero@gmail.com>
Fri, 5 Feb 2016 16:45:01 +0000 (16:45 +0000)
committerJordi <jordiguerrero@gmail.com>
Fri, 5 Feb 2016 16:45:01 +0000 (16:45 +0000)
Fresonbot.pyc
PID_test.py [new file with mode: 0644]
pinout.pyc
stop.py [new file with mode: 0644]

index 611c67e..2d6272e 100644 (file)
Binary files a/Fresonbot.pyc and b/Fresonbot.pyc differ
diff --git a/PID_test.py b/PID_test.py
new file mode 100644 (file)
index 0000000..7e83db9
--- /dev/null
@@ -0,0 +1,233 @@
+###
+# 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"
+
+subprocess.call("bashScripts/enable_encoder_slots.sh")
+
+GPIO.setup(PinMotorLeftPhase, GPIO.OUT)
+GPIO.setup(PinMotorRightPhase, GPIO.OUT)
+
+#fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
+#fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
+fTicksLeft = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
+fTicksRight = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
+
+TicksxRevolution = 1200
+WheelRadius = 16
+#WheelDistance = 85.5
+WheelDistance = 89
+
+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
+
+    distanceLeft = 2 * math.pi * WheelRadius * (float(IncTicksLeft) / TicksxRevolution)
+    distanceRight = 2 * math.pi * WheelRadius * (float(IncTicksRight) / TicksxRevolution)
+
+    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
+
+def changeHeading():
+    global u
+    global headingError
+    global headingErrorDerivative
+    global headingErrorIntegral
+    global lastHeadingError
+    global VelLeft
+    global VelRight
+
+    deltax = xTarget - x
+    deltay = yTarget - 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)))
+
+    print ('referenceHeading {0:.4f}'.format(referenceHeading))
+
+    headingError = referenceHeading - heading
+    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
+
+    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)
+
+    return
+
+def goTarget():
+    global start_dt
+    global dt
+    global xTarget
+    global yTarget
+
+    while ((x > xTarget + sensitivityLimit) or (x < xTarget - sensitivityLimit)) or ((y > yTarget + sensitivityLimit) or (y < yTarget - sensitivityLimit)):
+
+        end_dt = time.time() * 1000
+        dt = end_dt - start_dt
+    
+        print ('dt {0:.4f}'.format(dt))
+        print ('***********************')
+        start_dt = end_dt
+
+        measureDistance();
+
+        changeHeading();
+
+        motion();
+
+        time.sleep(0.02)
+    return
+
+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
+Kp = 150
+
+Ki = 0.1
+
+Kd = 0.001
+
+VelLeft = 50
+VelRight = 50
+#xTarget = int(sys.argv[1])
+#yTarget = int(sys.argv[2])
+
+getTicks();
+
+StartTicksLeft = TicksLeft 
+StartTicksRight = TicksRight 
+
+start_dt = time.time() * 1000
+
+#motion();
+GPIO.output(PinMotorLeftPhase, 0)
+GPIO.output(PinMotorRightPhase, 0)
+PWM.start(PinMotorLeftPwm,50)
+PWM.start(PinMotorRightPwm,50)
+
+time.sleep(0.02)
+
+measureDistance();
+
+it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
+#it = iter([200,0,200,200,0,200,0,0])
+#it = iter([200,0])
+for xT, yT in zip(it, it):
+    xTarget = xT
+    yTarget = yT
+    goTarget();
+
+stop();
+
+time.sleep(0.5)
+
+getTicks();
+
+measureDistance();
index bd38279..50329eb 100644 (file)
Binary files a/pinout.pyc and b/pinout.pyc differ
diff --git a/stop.py b/stop.py
new file mode 100644 (file)
index 0000000..647332e
--- /dev/null
+++ b/stop.py
@@ -0,0 +1,4 @@
+from Fresonbot import Fresonbot
+
+R1 = Fresonbot()
+R1.stop()