Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import numpy as np
- import matplotlib.pyplot as plt
- import turtle
- import time
- # Global params
- TIMER = 0
- TIME_STEP = 0.001
- SETPOINT = 100
- SIM_TIME = 100 # in seconds
- INITIAL_X = 0
- INITIAL_Y = -100
- MASS =1 # kg
- MAX_THRUST = 15 # Newtons
- Landing_Thrust = 30
- g = -9.81 # Gravitational constant
- V_i = 0 #initial velocity
- Y_i = 0 #initial height
- # ------------------
- # PID values
- # Fine tuning the PID gains
- # Zieglar Nicholas Method, the heuristic apporoach for fine tuning
- # ku = 0.5
- # Tu = 4 ms=iliseconds 0.0444
- # kp = 0.6*ku = 0.30
- # ki = (1.2 ku)/Tu(in seconds) = ((1.2*0.7)/4)*1000 = 13.513
- # kd = (3*ku*Tu)/40 = (3*0.7*(4/1000))/40 = 0.001665
- KP = 0.5
- KI = 13.513
- KD = 0.001665
- class Simulation(object):
- def __init__(self):
- self.Insight = Rocket()
- self.pid = PID(KP, KI, KD, SETPOINT)
- self.screen = turtle.Screen()
- self.screen.setup(800, 600)
- self.marker = turtle.Turtle()
- self.marker.penup()
- self.marker.left(180)
- self.marker.goto(15, SETPOINT)
- self.marker.color("red")
- self.sim = True
- self.timer = 0
- self.poses = np.array([])
- self.times = np.array([])
- def cycle(self):
- while(self.sim):
- thrust = self.pid.compute(self.Insight.get_y())
- #print(thrust)
- #print(self.target)
- self.Insight.set_ddy(thrust)
- self.Insight.set_dy()
- self.Insight.set_y()
- time.sleep(TIME_STEP)
- self.timer += TIME_STEP
- if self.timer > SIM_TIME:
- self.sim = False
- print("Time limit reached")
- elif self.Insight.get_y() > 800:
- self.sim = False
- print("Upper ceiling hit")
- elif self.Insight.get_y() < -800:
- self.sim = False
- print("Lower ceiling hit")
- #print(self.Insight.get_y(), self.target)
- if 100 <= self.Insight.get_y() <= 102:
- #print(" Target achieved, returning to base")
- self.pid.setpoint = 20
- self.pid.integral_error = 0 # integral accumulates all the values throught the run, reset it
- self.pid.error_last = self.pid.error # reset error, as it still has the last value from setpoint 100
- #print(SETPOINT)
- self.poses = np.append(self.poses, self.Insight.get_y())
- self.times = np.append(self.times, self.timer)
- #print(self.Insight.get_y)
- graph(self.times, self.poses)
- def graph(x, y):
- plt.plot(x,y)
- plt.show()
- class Rocket(object):
- def __init__(self):
- global Rocket
- self.Rocket = turtle.Turtle()
- self.Rocket.shape('square')
- self.Rocket.color('black')
- self.Rocket.penup()
- self.Rocket.goto(INITIAL_X, INITIAL_Y)
- self.Rocket.speed(0)
- #physics
- self.ddy = 0
- self.dy = V_i
- self.y = INITIAL_Y
- def set_ddy(self, thrust):
- self.ddy = g + thrust / MASS
- def get_ddy(self):
- return self.ddy
- def set_dy(self):
- self.dy += self.ddy * TIME_STEP
- def get_dy(self):
- return self.dy
- def set_y(self):
- self.y += self.dy
- self.Rocket.sety(self.y)
- def get_y(self):
- self.y = self.Rocket.ycor()
- return self.y
- class PID(object):
- def __init__(self, KP, KI, KD, target):
- self.kp = KP
- self.ki = KI
- self.kd = KD
- self.setpoint = target
- self.error = 0
- self.integral_error = 0
- self.error_last = 0
- self.derivative_error = 0
- self.output = 0
- def compute(self, pos):
- self.error = self.setpoint - pos # error = target height - current position height
- self.integral_error += self.error * TIME_STEP # adding error wrt to time
- self.derivative_error = (self.error - self.error_last) / TIME_STEP
- self.error_last = self.error
- self.output = self.kp*self.error + self.ki*self.integral_error + self.kd*self.derivative_error
- #print(self.setpoint)
- #print(self.integral_error)
- if self.output >= MAX_THRUST:
- if self.setpoint == 20 and pos <= 50:
- self.output = Landing_Thrust
- if 19 <= pos <= 21:
- self.output = MAX_THRUST
- else: # Saturation responses, setting the value of the thurst to the maximum physically possible value
- self.output = MAX_THRUST # If the PID calculates the thurst to be a value which is physically impossible to have
- if self.setpoint == 20 and pos > 21:
- self.output = -10
- elif self.output <= 0:
- self.output = 0
- return self.output
- def main(): # 1 DOF PID contoller
- sim = Simulation()
- sim.cycle()
- #turtle.done()
- main()
Advertisement
Add Comment
Please, Sign In to add comment