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 subprocess.call("bashScripts/enable_encoder_slots.sh") # Enabling encoders
12
13 GPIO.setup(pinout.PinMotorLeftPhase, GPIO.OUT)
14 GPIO.setup(pinout.PinMotorRightPhase, GPIO.OUT)
15
16 fTicksLeft = "/sys/devices/ocp.3/48302000.epwmss/48302180.eqep/position"
17 fTicksRight = "/sys/devices/ocp.3/48304000.epwmss/48304180.eqep/position"
18
19 TicksxRevolution = 1200 # Depending on the encoder
20
21 ## you have to take this measures accurately
22 WheelRadius = 31.8/2 # I took the diameter and divided by 2
23 WheelDistance = 88.9 # between centers
24
25 #global distance
26 robot1 = Fresonbot() # Using Fresonbot class
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     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.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax))) * -1)
44     if ((deltay >= 0) and (deltax < 0)):
45         referenceHeading = math.acos(deltax / math.sqrt((deltay * deltay) + (deltax * deltax)))
46     if ((deltay < 0) and (deltax >= 0)):
47         referenceHeading = math.asin(deltay / math.sqrt((deltay * deltay) + (deltax * deltax)))
48
49     print ('referenceHeading {0:.4f}'.format(referenceHeading))
50
51     headingError = referenceHeading - robot1.heading
52     print ('headingError {0:.4f}'.format(headingError))
53     print ('deltax {0:.4f}'.format(deltax))
54     print ('deltay {0:.4f}'.format(deltay))
55
56     headingErrorDerivative = (headingError - lastHeadingError) / dt
57     headingErrorIntegral += (headingError * dt)
58     lastHeadingError = headingError
59
60     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
61
62     VelLeft = (velocityTranslational + u)
63     VelRight = (velocityTranslational - u)
64
65     return
66
67 def goTarget():
68     global start_dt
69     global dt
70     global xTarget
71     global yTarget
72
73     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
74
75         end_dt = time.time() * 1000
76         dt = end_dt - start_dt
77     
78         print ('dt {0:.4f}'.format(dt))
79         print ('***********************')
80         start_dt = end_dt
81
82         robot1.getPosition();
83
84         changeHeading();
85
86         robot1.motion(VelLeft, VelRight)
87
88         time.sleep(0.02)
89     return
90
91 IncTicksLeft = 0
92 IncTicksRight = 0
93 #x = 0
94 #y = 0
95 #heading = 0
96 distanceLeft = 0
97 distanceRight = 0
98 #distance = 0
99 lastHeadingError = 0
100 headingErrorIntegral = 0
101 velocityTranslational = 50
102 sensitivityLimit = 5
103 u = 0
104
105 # PID constants
106 Kp = 150
107 Ki = 0.1
108 Kd = 0.001
109
110 VelLeft = 50
111 VelRight = 50
112
113 robot1.getTicks();
114
115 #StartTicksLeft = robot1.TicksLeft 
116 #StartTicksRight = robot1.TicksRight 
117
118 start_dt = time.time() * 1000
119
120 #motion();
121 robot1.motion(velocityTranslational,velocityTranslational)
122
123 time.sleep(0.02)
124
125 robot1.getPosition();
126
127 #it = iter([400,0,400,400,0,400,200,200,0,0,400,0,400,400,0,400,200,200,0,0])
128 #it = iter([400,0,400,400,0,400,200,200,0,0])
129 it = iter([200,0,200,200,0,200,0,0])
130 #it = iter([100,0])
131 for xT, yT in zip(it, it):
132     xTarget = xT
133     yTarget = yT
134     goTarget();
135
136 robot1.stop();
137
138 time.sleep(0.5)
139
140 robot1.getTicks();
141
142 robot1.getPosition();