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