Editing PIDtest
[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 #    referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
47
48     referenceHeading =  math.atan2(deltay, deltax)
49  #   referenceHeading = (referenceHeading + math.pi)/(2*math.pi)-math.pi
50
51     print ('referenceHeading {0:.4f}'.format(referenceHeading))
52
53     headingError = referenceHeading - robot1.heading
54     print ('headingError {0:.4f}'.format(headingError))
55     print ('deltax {0:.4f}'.format(deltax))
56     print ('deltay {0:.4f}'.format(deltay))
57
58     headingErrorDerivative = (headingError - lastHeadingError) / dt
59     headingErrorIntegral += (headingError * dt)
60     lastHeadingError = headingError
61
62     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
63
64     VelLeft = (velocityTranslational + u)
65     VelRight = (velocityTranslational - u)
66
67     return
68
69 def goTarget():
70     global start_dt
71     global dt
72     global xTarget
73     global yTarget
74
75     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
76
77         end_dt = time.time() * 1000
78         dt = end_dt - start_dt
79     
80         print ('dt {0:.4f}'.format(dt))
81         print ('***********************')
82         start_dt = end_dt
83
84         robot1.getPosition();
85
86         changeHeading();
87
88         robot1.motion(VelLeft, VelRight)
89
90         time.sleep(0.02)
91     return
92
93 #Initializing variables
94 IncTicksLeft = 0
95 IncTicksRight = 0
96 distanceLeft = 0
97 distanceRight = 0
98 lastHeadingError = 0
99 headingErrorIntegral = 0
100 velocityTranslational = 50
101 sensitivityLimit = 5
102 u = 0
103 VelLeft = 50
104 VelRight = 50
105
106 #Main program
107 robot1.getTicks();
108
109 start_dt = time.time() * 1000
110
111 robot1.motion(velocityTranslational,velocityTranslational)
112
113 time.sleep(0.02)
114
115 robot1.getPosition();
116
117 for xT, yT in zip(it, it):
118     xTarget = xT
119     yTarget = yT
120
121     goTarget();
122
123 robot1.stop();
124
125 time.sleep(0.5)
126
127 robot1.getTicks();
128
129 robot1.getPosition();