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