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