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 it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
15 #it = iter([400,0,400,400,0,400,200,200,0,0])
16 #it = iter([200,0,200,200,0,200,0,0])
17 #it = iter([100,0])
18
19 # PID constants
20 Kp = 150
21 Ki = 0.1
22 Kd = 0.001
23
24 def changeHeading():
25     global u
26     global headingError
27     global headingErrorDerivative
28     global headingErrorIntegral
29     global lastHeadingError
30     global VelLeft
31     global VelRight
32
33     deltax = xTarget - robot1.x
34     deltay = yTarget - robot1.y
35
36     if ((deltay >= 0) and (deltax >= 0)):
37         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
38     if ((deltay < 0) and (deltax < 0)):
39         referenceHeading = (math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
40     if ((deltay >= 0) and (deltax < 0)):
41         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
42     if ((deltay < 0) and (deltax >= 0)):
43         referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
44
45     print ('referenceHeading {0:.4f}'.format(referenceHeading))
46
47     headingError = referenceHeading - robot1.heading
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 IncTicksLeft = 0
88 IncTicksRight = 0
89
90 distanceLeft = 0
91 distanceRight = 0
92
93 lastHeadingError = 0
94 headingErrorIntegral = 0
95 velocityTranslational = 50
96 sensitivityLimit = 5
97 u = 0
98
99 VelLeft = 50
100 VelRight = 50
101
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     goTarget();
116
117 robot1.stop();
118
119 time.sleep(0.5)
120
121 robot1.getTicks();
122
123 robot1.getPosition();