d31bdecbc99b0be4ffd9db7c1a6ca1daaf26acf9
[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 #global distance
12 robot1 = Fresonbot() # Using Fresonbot class
13
14 # Use it variable to describe your trajectory ([x1,y2,x2,y2,....,xn,yn])
15 it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
16 #it = iter([400,0,400,400,0,400,200,200,0,0])
17 #it = iter([200,0,200,200,0,200,0,0])
18 #it = iter([100,0])
19
20 # PID constants
21 Kp = 150
22 Ki = 0.1
23 Kd = 0.001
24
25 def changeHeading():
26     global u
27     global headingError
28     global headingErrorDerivative
29     global headingErrorIntegral
30     global lastHeadingError
31     global VelLeft
32     global VelRight
33
34     deltax = xTarget - robot1.x
35     deltay = yTarget - robot1.y
36
37     if ((deltay >= 0) and (deltax >= 0)):
38         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
39     if ((deltay < 0) and (deltax < 0)):
40         referenceHeading = (math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
41     if ((deltay >= 0) and (deltax < 0)):
42         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
43     if ((deltay < 0) and (deltax >= 0)):
44         referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
45
46     print ('referenceHeading {0:.4f}'.format(referenceHeading))
47
48     headingError = referenceHeading - robot1.heading
49     print ('headingError {0:.4f}'.format(headingError))
50     print ('deltax {0:.4f}'.format(deltax))
51     print ('deltay {0:.4f}'.format(deltay))
52
53     headingErrorDerivative = (headingError - lastHeadingError) / dt
54     headingErrorIntegral += (headingError * dt)
55     lastHeadingError = headingError
56
57     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
58
59     VelLeft = (velocityTranslational + u)
60     VelRight = (velocityTranslational - u)
61
62     return
63
64 def goTarget():
65     global start_dt
66     global dt
67     global xTarget
68     global yTarget
69
70     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
71
72         end_dt = time.time() * 1000
73         dt = end_dt - start_dt
74     
75         print ('dt {0:.4f}'.format(dt))
76         print ('***********************')
77         start_dt = end_dt
78
79         robot1.getPosition();
80
81         changeHeading();
82
83         robot1.motion(VelLeft, VelRight)
84
85         time.sleep(0.02)
86     return
87
88 #Initializing variables
89 IncTicksLeft = 0
90 IncTicksRight = 0
91 distanceLeft = 0
92 distanceRight = 0
93 lastHeadingError = 0
94 headingErrorIntegral = 0
95 velocityTranslational = 50
96 sensitivityLimit = 5
97 u = 0
98 VelLeft = 50
99 VelRight = 50
100
101 #Main program
102 robot1.getTicks();
103
104 start_dt = time.time() * 1000
105
106 robot1.motion(velocityTranslational,velocityTranslational)
107
108 time.sleep(0.02)
109
110 robot1.getPosition();
111
112 for xT, yT in zip(it, it):
113     xTarget = xT
114     yTarget = yT
115
116     goTarget();
117
118 robot1.stop();
119
120 time.sleep(0.5)
121
122 robot1.getTicks();
123
124 robot1.getPosition();