changes in eqep2
[fresonbot.git] / 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,100,100,0,100,0,0])
18 #it = iter([200,100])
19
20 # PID constants
21 #Kp = 150
22 #Ki = 0.1
23 #Kd = 0.001
24 Kp = 200
25 Ki = 0.0
26 Kd = 0.0
27
28 #Change velocityTranslational to increase or decrease velocity
29 velocityTranslational = 70 
30
31 def changeHeading():
32     global u
33     global headingError
34     global headingErrorDerivative
35     global headingErrorIntegral
36     global lastHeadingError
37     global VelLeft
38     global VelRight
39
40     deltax = xTarget - robot1.x
41     deltay = yTarget - robot1.y
42
43     referenceHeading =  math.atan2(deltay, deltax)
44 #    headingError = referenceHeading - robot1.heading
45     headingError = (referenceHeading - robot1.heading+ math.pi)%(2*math.pi) - math.pi
46
47     headingErrorDerivative = (headingError - lastHeadingError) / dt
48     headingErrorIntegral += (headingError * dt)
49     lastHeadingError = headingError
50
51     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
52
53     VelLeft = (velocityTranslational + u)
54     VelRight = (velocityTranslational - u)
55
56     return
57
58 def goTarget():
59     global start_dt
60     global dt
61     global xTarget
62     global yTarget
63
64     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
65
66         end_dt = time.time() * 1000
67         dt = end_dt - start_dt
68         start_dt = end_dt
69
70         robot1.getPosition();
71
72         changeHeading();
73
74         robot1.motion(VelLeft, VelRight)
75
76         time.sleep(0.02)
77     return
78
79 #Initializing variables
80 IncTicksLeft = 0
81 IncTicksRight = 0
82 distanceLeft = 0
83 distanceRight = 0
84 lastHeadingError = 0
85 headingErrorIntegral = 0
86 sensitivityLimit = 5
87 u = 0
88 VelLeft = velocityTranslational
89 VelRight = velocityTranslational
90
91 #Main program
92 robot1.getTicks();
93
94 start_dt = time.time() * 1000
95
96 robot1.motion(velocityTranslational,velocityTranslational)
97
98 time.sleep(0.02)
99
100 robot1.getPosition();
101
102 for xT, yT in zip(it, it):
103     xTarget = xT
104     yTarget = yT
105
106     goTarget();
107
108 robot1.stop();
109
110 time.sleep(0.5)
111
112 robot1.getTicks();
113
114 robot1.getPosition();