PID_test.py

   1 import Adafruit_BBIO.PWM as PWM
   2 import Adafruit_BBIO.GPIO as GPIO
   3
   4 import time
   5 import math
   6 import subprocess
   7
   8 import pinout # pinout.py is where I defined all used pins in BBB
   9 from Fresonbot import Fresonbot
  10
  11 robot1 = Fresonbot() # Using Fresonbot class
  12
  13 # Use it variable to describe your trajectory ([x1,y2,x2,y2,....,xn,yn])
  14 #it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
  15 #it = iter([400,0,400,400,0,400,200,200,0,0])
  16 it = iter([200,0,200,200,0,200,0,0])
  17 #it = iter([100,0])
  18
  19 # PID constants
  20 #Kp = 150
  21 Kp = 200
  22 Ki = 0.1
  23 Kd = 0.001
  24 #Change velocityTranslational to increase or decrease velocity
  25 velocityTranslational = 70
  26
  27 def changeHeading():
  28     global u
  29     global headingError
  30     global headingErrorDerivative
  31     global headingErrorIntegral
  32     global lastHeadingError
  33     global VelLeft
  34     global VelRight
  35
  36     deltax = xTarget - robot1.x
  37     deltay = yTarget - robot1.y
  38
  39     referenceHeading =  math.atan2(deltay, deltax)
  40 #    headingError = referenceHeading - robot1.heading
  41     headingError = (referenceHeading - robot1.heading+ math.pi)%(2*math.pi) - math.pi
  42
  43     headingErrorDerivative = (headingError - lastHeadingError) / dt
  44     headingErrorIntegral += (headingError * dt)
  45     lastHeadingError = headingError
  46
  47     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
  48
  49     VelLeft = (velocityTranslational + u)
  50     VelRight = (velocityTranslational - u)
  51
  52     return
  53
  54 def goTarget():
  55     global start_dt
  56     global dt
  57     global xTarget
  58     global yTarget
  59
  60     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
  61
  62         end_dt = time.time() * 1000
  63         dt = end_dt - start_dt
  64         start_dt = end_dt
  65
  66         robot1.getPosition();
  67
  68         changeHeading();
  69
  70         robot1.motion(VelLeft, VelRight)
  71
  72         time.sleep(0.02)
  73     return
  74
  75 #Initializing variables
  76 IncTicksLeft = 0
  77 IncTicksRight = 0
  78 distanceLeft = 0
  79 distanceRight = 0
  80 lastHeadingError = 0
  81 headingErrorIntegral = 0
  82 sensitivityLimit = 5
  83 u = 0
  84 VelLeft = velocityTranslational
  85 VelRight = velocityTranslational
  86
  87 #Main program
  88 robot1.getTicks();
  89
  90 start_dt = time.time() * 1000
  91
  92 robot1.motion(velocityTranslational,velocityTranslational)
  93
  94 time.sleep(0.02)
  95
  96 robot1.getPosition();
  97
  98 for xT, yT in zip(it, it):
  99     xTarget = xT
 100     yTarget = yT
 101
 102     goTarget();
 103
 104 robot1.stop();
 105
 106 time.sleep(0.5)
 107
 108 robot1.getTicks();
 109
 110 robot1.getPosition();