a167e924474c25366ae1bc00c71cb1660cc7d53c
[fresonbot.git] / PID_test.py
1 ###
2 # arg1 xTarget
3 # arg2 yTarget
4
5 import Adafruit_BBIO.PWM as PWM
6 import Adafruit_BBIO.GPIO as GPIO
7
8 import time
9 import sys
10 import math
11 import subprocess
12
13 PinMotorLeftPwm = "P9_14"
14 PinMotorRightPwm = "P9_16"
15 PinMotorLeftPhase = "P9_12"
16 PinMotorRightPhase = "P9_23"
17
18 subprocess.call("bashScripts/enable_encoder_slots.sh")
19
20 GPIO.setup(PinMotorLeftPhase, GPIO.OUT)
21 GPIO.setup(PinMotorRightPhase, GPIO.OUT)
22
23 fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
24 fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
25
26 TicksxRevolution = 1200
27 #WheelRadius = 16
28 WheelRadius = 31.8/2
29 #WheelDistance = 85.5
30 #WheelDistance = 89
31 WheelDistance = 88.9
32
33 global distance
34
35 def motion():
36     AbsVelLeft = abs(VelLeft)
37     AbsVelRight = abs(VelRight)
38     if (VelLeft < 0):
39         PhaseLeft = 1
40     else:
41         PhaseLeft = 0
42     if (VelRight < 0):
43         PhaseRight = 1
44     else:
45         PhaseRight = 0
46     if (AbsVelLeft > 100):
47         AbsVelLeft = 100
48     if (AbsVelRight > 100):
49         AbsVelRight = 100
50
51     GPIO.output(PinMotorLeftPhase, PhaseLeft)
52     GPIO.output(PinMotorRightPhase, PhaseRight)
53     PWM.set_duty_cycle(PinMotorLeftPwm,AbsVelLeft)
54     PWM.set_duty_cycle(PinMotorRightPwm,AbsVelRight)
55     return 
56
57 def getTicks():
58     global TicksLeft
59     global TicksRight
60     foTicksLeft = open(fTicksLeft, "r")
61     foTicksRight = open(fTicksRight, "r")
62
63     TicksLeft = foTicksLeft.read()
64     TicksLeft = int(TicksLeft.split('\n', 1)[0])
65     TicksRight = foTicksRight.read()
66     TicksRight = int(TicksRight.split('\n', 1)[0])
67
68     foTicksLeft.close()
69     foTicksRight.close()
70     return
71
72 def stop():
73     GPIO.output(PinMotorLeftPhase,0)
74     GPIO.output(PinMotorRightPhase,0)
75     PWM.start(PinMotorLeftPwm,0)
76     PWM.start(PinMotorRightPwm,0)
77     GPIO.cleanup()
78     return
79
80 def measureDistance():
81     global distance
82     global heading
83     global x
84     global y
85     global StartTicksLeft 
86     global StartTicksRight
87
88     getTicks();
89     EndTicksLeft = TicksLeft
90     EndTicksRight = TicksRight
91
92     IncTicksLeft = EndTicksLeft - StartTicksLeft
93     IncTicksRight = EndTicksRight - StartTicksRight
94
95     distanceLeft = 2 * math.pi * WheelRadius * (float(IncTicksLeft) / TicksxRevolution)
96     distanceRight = 2 * math.pi * WheelRadius * (float(IncTicksRight) / TicksxRevolution)
97
98     newdistance = (distanceLeft + distanceRight) / 2
99     distance += newdistance
100
101     heading += (distanceLeft - distanceRight) / WheelDistance
102     x += newdistance * math.cos(heading)
103     y += newdistance * math.sin(heading)
104     print ('heading {0:.4f}'.format(heading))
105     print ('x {0:.4f}'.format(x))
106     print ('y {0:.4f}'.format(y))
107
108     StartTicksLeft = EndTicksLeft
109     StartTicksRight = EndTicksRight
110
111     return
112
113 def changeHeading():
114     global u
115     global headingError
116     global headingErrorDerivative
117     global headingErrorIntegral
118     global lastHeadingError
119     global VelLeft
120     global VelRight
121
122     deltax = xTarget - x
123     deltay = yTarget - y
124
125     if ((deltay >= 0) and (deltax >= 0)):
126         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
127     if ((deltay < 0) and (deltax < 0)):
128         referenceHeading = (math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
129     if ((deltay >= 0) and (deltax < 0)):
130         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
131     if ((deltay < 0) and (deltax >= 0)):
132         referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
133
134     print ('referenceHeading {0:.4f}'.format(referenceHeading))
135
136     headingError = referenceHeading - heading
137     print ('headingError {0:.4f}'.format(headingError))
138     print ('deltax {0:.4f}'.format(deltax))
139     print ('deltay {0:.4f}'.format(deltay))
140
141     headingErrorDerivative = (headingError - lastHeadingError) / dt
142     headingErrorIntegral += (headingError * dt)
143     lastHeadingError = headingError
144
145     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
146
147 #    VelLeft = (2*velocityTranslational + u*WheelDistance) / (2*WheelRadius)
148 #    VelRight = (2*velocityTranslational - u*WheelDistance) / (2*WheelRadius)
149     VelLeft = (velocityTranslational + u)
150     VelRight = (velocityTranslational - u)
151
152     return
153
154 def goTarget():
155     global start_dt
156     global dt
157     global xTarget
158     global yTarget
159
160     while ((x > xTarget + sensitivityLimit) or (x < xTarget - sensitivityLimit)) or ((y > yTarget + sensitivityLimit) or (y < yTarget - sensitivityLimit)):
161
162         end_dt = time.time() * 1000
163         dt = end_dt - start_dt
164     
165         print ('dt {0:.4f}'.format(dt))
166         print ('***********************')
167         start_dt = end_dt
168
169         measureDistance();
170
171         changeHeading();
172
173         motion();
174
175         time.sleep(0.02)
176     return
177
178 IncTicksLeft = 0
179 IncTicksRight = 0
180 x = 0
181 y = 0
182 heading = 0
183 distanceLeft = 0
184 distanceRight = 0
185 distance = 0
186 lastHeadingError = 0
187 headingErrorIntegral = 0
188 velocityTranslational = 50
189 sensitivityLimit = 5
190 u = 0
191 Kp = 150
192
193 Ki = 0.1
194
195 Kd = 0.001
196
197 VelLeft = 50
198 VelRight = 50
199 #xTarget = int(sys.argv[1])
200 #yTarget = int(sys.argv[2])
201
202 getTicks();
203
204 StartTicksLeft = TicksLeft 
205 StartTicksRight = TicksRight 
206
207 start_dt = time.time() * 1000
208
209 #motion();
210 GPIO.output(PinMotorLeftPhase, 0)
211 GPIO.output(PinMotorRightPhase, 0)
212 PWM.start(PinMotorLeftPwm,50)
213 PWM.start(PinMotorRightPwm,50)
214
215 time.sleep(0.02)
216
217 measureDistance();
218
219 it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
220 #it = iter([400,0,400,400,0,400,200,200,0,0])
221 #it = iter([200,0,200,200,0,200,0,0])
222 #it = iter([200,50])
223 for xT, yT in zip(it, it):
224     xTarget = xT
225     yTarget = yT
226     goTarget();
227
228 stop();
229
230 time.sleep(0.5)
231
232 getTicks();
233
234 measureDistance();