├── README.md
└── PID_Video.py


/README.md:
--------------------------------------------------------------------------------
 1 | # PIDtest
 2 | 
 3 | Note: this was created in one 3 hour session and uploaded to youtube (video link below)
 4 | Thanks to those who have contributed so far :) 
 5 | 
 6 | <h1> I MADE AN IMPROVED VERSION OF THIS CODE https://github.com/Jmack66/TVC-Sim </h1>
 7 | 
 8 | a Toy implementation of a PID controller in Python using turtle for graphics 
 9 | video found here https://youtu.be/ZMI_kpNUgJM
10 | 


--------------------------------------------------------------------------------
/PID_Video.py:
--------------------------------------------------------------------------------
  1 | import numpy as np
  2 | import matplotlib.pyplot as plt
  3 | import turtle 
  4 | import time
  5 | 
  6 | #GLOBAL PARAMS
  7 | TIMER = 0
  8 | TIME_STEP = 0.001
  9 | SETPOINT = 10
 10 | SIM_TIME = 100
 11 | INITIAL_X = 0
 12 | INITIAL_Y = -100
 13 | MASS = 1 #kg
 14 | MAX_THRUST = 15 #Newtons
 15 | g = -9.81 #Gravitational constant
 16 | V_i = 0 #initial velocity 
 17 | Y_i = 0 #initial height
 18 | #------------
 19 | #---PID GAINS--- 
 20 | #ku = 0.6
 21 | #Tu = 18 ms
 22 | KP = 0.36
 23 | KI = 40.0
 24 | KD = 0.0008099999999999997
 25 | #KD = 0.00128 for higher setpoints
 26 | antiWindup = True
 27 | # KP = 0.6
 28 | # KI = 0.0
 29 | # KD = 0.0
 30 | #---------------
 31 | 
 32 | class Simulation(object):
 33 | 	def __init__(self):
 34 | 		self.Insight = Rocket()
 35 | 		self.pid = PID(KP,KI,KD,SETPOINT)
 36 | 		self.screen = turtle.Screen()
 37 | 		self.screen.setup(800,600)
 38 | 		self.marker = turtle.Turtle()
 39 | 		self.marker.penup()
 40 | 		self.marker.left(180)
 41 | 		self.marker.goto(15,SETPOINT)
 42 | 		self.marker.color('red')
 43 | 		self.sim = True
 44 | 		self.timer = 0
 45 | 		self.poses = np.array([])
 46 | 		self.times = np.array([])
 47 | 		self.kpe = np.array([])
 48 | 		self.kde = np.array([])
 49 | 		self.kie = np.array([])
 50 | 		self.thrst = np.array([])
 51 | 	def cycle(self):
 52 | 		while(self.sim):
 53 | 			thrust = self.pid.compute(self.Insight.get_y())
 54 | 			print(thrust)
 55 | 			self.Insight.set_ddy(thrust)
 56 | 			self.Insight.set_dy()
 57 | 			self.Insight.set_y()
 58 | 			time.sleep(TIME_STEP)
 59 | 			self.timer += 1
 60 | 			if self.timer > SIM_TIME:
 61 | 				print("SIM ENDED")
 62 | 				self.sim = False
 63 | 			elif self.Insight.get_y() > 700:
 64 | 				print("OUT OF BOUNDS")
 65 | 				self.sim = False
 66 | 			elif self.Insight.get_y() < -700:
 67 | 				print("OUT OF BOUNDS")
 68 | 				self.sim = False
 69 | 			self.poses = np.append(self.poses,self.Insight.get_y())
 70 | 			self.times = np.append(self.times,self.timer)
 71 | 			self.kpe = np.append(self.kpe,self.pid.get_kpe())
 72 | 			self.kde = np.append(self.kde,self.pid.get_kde())
 73 | 			self.kie = np.append(self.kie,self.pid.get_kie())
 74 | 			self.thrst = np.append(self.thrst,thrust)
 75 | 		graph(self.times,self.poses,self.kpe,self.kde,self.kie,self.thrst)
 76 | 
 77 | def graph(x,y1,y2,y3,y4,y5):
 78 | 	fig, (ax1, ax2,ax3,ax4,ax5) = plt.subplots(5, sharex=True)
 79 | 	#fig.suptitle('antiwindup')
 80 | 	ax1.set(ylabel='rocket \nHeight')
 81 | 	ax1.plot(x,y1)
 82 | 	ax2.set(ylabel='KP_error')
 83 | 	ax2.plot(x,y2,'tab:red')
 84 | 	ax3.set(ylabel='KD_error')
 85 | 	ax3.plot(x,y3,'tab:orange')
 86 | 	ax4.set(ylabel='KI_error')
 87 | 	ax4.plot(x,y4,'tab:pink')
 88 | 	ax5.set(ylabel='rocket \nThrust')
 89 | 	ax5.plot(x,y5,'tab:brown')
 90 | 	plt.show()
 91 | 
 92 | class Rocket(object):
 93 | 	def __init__(self):
 94 | 		global Rocket
 95 | 		self.Rocket = turtle.Turtle()
 96 | 		self.Rocket.shape('square')
 97 | 		self.Rocket.color('black')
 98 | 		self.Rocket.penup()
 99 | 		self.Rocket.goto(INITIAL_X,INITIAL_Y)
100 | 		self.Rocket.speed(0)
101 | 		#physics
102 | 		self.ddy = 0
103 | 		self.dy = V_i
104 | 		self.y = INITIAL_Y
105 | 	def set_ddy(self,thrust):
106 | 		self.ddy = g + thrust / MASS
107 | 	def get_ddy(self):
108 | 		return self.ddy
109 | 	def set_dy(self):
110 | 		self.dy += self.ddy * TIME_STEP
111 | 	def get_dy(self):
112 | 		return self.dy
113 | 	def set_y(self):
114 | 		self.Rocket.sety(self.y + self.dy * TIME_STEP)
115 | 	def get_y(self):
116 | 		self.y = self.Rocket.ycor()
117 | 		return self.y
118 | 
119 | class PID(object):
120 | 	def __init__(self,KP,KI,KD,target):
121 | 		self.kp = KP
122 | 		self.ki = KI
123 | 		self.kd = KD 
124 | 		self.setpoint = target
125 | 		self.error = 0
126 | 		self.integral_error = 0
127 | 		self.error_last = 0
128 | 		self.derivative_error = 0
129 | 		self.output = 0
130 | 	def compute(self, pos):
131 | 		self.error = self.setpoint - pos
132 | 		#self.integral_error += self.error * TIME_STEP
133 | 		self.derivative_error = (self.error - self.error_last) / TIME_STEP
134 | 		self.error_last = self.error
135 | 		self.output = self.kp*self.error + self.ki*self.integral_error + self.kd*self.derivative_error
136 | 		if(abs(self.output)>= MAX_THRUST and (((self.error>=0) and (self.integral_error>=0))or((self.error<0) and (self.integral_error<0)))):
137 | 			if(antiWindup):
138 | 				#no integration
139 | 				self.integral_error = self.integral_error
140 | 			else:
141 | 				#if no antiWindup rectangular integration
142 | 				self.integral_error += self.error * TIME_STEP
143 | 		else:
144 | 			#rectangular integration
145 | 			self.integral_error += self.error * TIME_STEP
146 | 		if self.output >= MAX_THRUST:
147 | 			self.output = MAX_THRUST
148 | 		elif self.output <= 0:
149 | 			self.output = 0
150 | 		return self.output
151 | 		
152 | 	def get_kpe(self):
153 | 		return self.kp*self.error
154 | 	def get_kde(self):
155 | 		return self.kd*self.derivative_error
156 | 	def get_kie(self):
157 | 		return self.ki*self.integral_error
158 | 
159 | def main():
160 | 	sim = Simulation()
161 | 	sim.cycle()
162 | 
163 | main()
164 | 


--------------------------------------------------------------------------------