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 robot1 = Fresonbot() # Using Fresonbot class
12
13 # Use it variable to describe your trajectory ([x1,y2,x2,y2,....,xn,yn])
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 Kp = 200
22 Ki = 0.1
23 Kd = 0.001
24 #Change velocityTranslational to increase or decrease velocity
25 velocityTranslational = 70 
26
27 def changeHeading():
28     global u
29     global headingError
30     global headingErrorDerivative
31     global headingErrorIntegral
32     global lastHeadingError
33     global VelLeft
34     global VelRight
35
36     deltax = xTarget - robot1.x
37     deltay = yTarget - robot1.y
38
39     referenceHeading =  math.atan2(deltay, deltax)
40 #    headingError = referenceHeading - robot1.heading
41     headingError = (referenceHeading - robot1.heading+ math.pi)%(2*math.pi) - math.pi
42
43     headingErrorDerivative = (headingError - lastHeadingError) / dt
44     headingErrorIntegral += (headingError * dt)
45     lastHeadingError = headingError
46
47     u = (Kp * headingError) + (Ki * headingErrorIntegral) + (Kd * headingErrorDerivative)
48
49     VelLeft = (velocityTranslational + u)
50     VelRight = (velocityTranslational - u)
51
52     return
53
54 def goTarget():
55     global start_dt
56     global dt
57     global xTarget
58     global yTarget
59
60     while ((robot1.x > xTarget + sensitivityLimit) or (robot1.x < xTarget - sensitivityLimit)) or ((robot1.y > yTarget + sensitivityLimit) or (robot1.y < yTarget - sensitivityLimit)):
61
62         end_dt = time.time() * 1000
63         dt = end_dt - start_dt
64         start_dt = end_dt
65
66         robot1.getPosition();
67
68         changeHeading();
69
70         robot1.motion(VelLeft, VelRight)
71
72         time.sleep(0.02)
73     return
74
75 #Initializing variables
76 IncTicksLeft = 0
77 IncTicksRight = 0
78 distanceLeft = 0
79 distanceRight = 0
80 lastHeadingError = 0
81 headingErrorIntegral = 0
82 sensitivityLimit = 5
83 u = 0
84 VelLeft = velocityTranslational
85 VelRight = velocityTranslational
86
87 #Main program
88 robot1.getTicks();
89
90 start_dt = time.time() * 1000
91
92 robot1.motion(velocityTranslational,velocityTranslational)
93
94 time.sleep(0.02)
95
96 robot1.getPosition();
97
98 for xT, yT in zip(it, it):
99     xTarget = xT
100     yTarget = yT
101
102     goTarget();
103
104 robot1.stop();
105
106 time.sleep(0.5)
107
108 robot1.getTicks();
109
110 robot1.getPosition();