1 import Adafruit_BBIO.PWM as PWM
2 import Adafruit_BBIO.GPIO as GPIO
8 import pinout # pinout.py is where I defined all used pins in BBB
9 from Fresonbot import Fresonbot
11 robot1 = Fresonbot() # Using Fresonbot class
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])
24 #Change velocityTranslational to increase or decrease velocity
25 velocityTranslational = 70
30 global headingErrorDerivative
31 global headingErrorIntegral
32 global lastHeadingError
36 deltax = xTarget - robot1.x
37 deltay = yTarget - robot1.y
39 referenceHeading = math.atan2(deltay, deltax)
40 # headingError = referenceHeading - robot1.heading
41 headingError = (referenceHeading - robot1.heading+ math.pi)%(2*math.pi) - math.pi
43 headingErrorDerivative = (headingError - lastHeadingError) / dt
44 headingErrorIntegral += (headingError * dt)
45 lastHeadingError = headingError
47 u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
49 VelLeft = (velocityTranslational + u)
50 VelRight = (velocityTranslational - u)
60 while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
62 end_dt = time.time() * 1000
63 dt = end_dt - start_dt
70 robot1.motion(VelLeft, VelRight)
75 #Initializing variables
81 headingErrorIntegral = 0
84 VelLeft = velocityTranslational
85 VelRight = velocityTranslational
90 start_dt = time.time() * 1000
92 robot1.motion(velocityTranslational,velocityTranslational)
98 for xT, yT in zip(it, it):
110 robot1.getPosition();