Guest User

Untitled

a guest
Sep 19th, 2026
47
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 5.13 KB | None | 0 0
  1.  
  2.  
  3. import numpy as np
  4. import matplotlib.pyplot as plt
  5. import turtle
  6. import time
  7.  
  8. # Global params
  9. TIMER = 0
  10. TIME_STEP = 0.001
  11. SETPOINT = 100
  12. SIM_TIME = 100  # in seconds
  13. INITIAL_X = 0
  14. INITIAL_Y = -100
  15. MASS =1        # kg
  16. MAX_THRUST = 15 # Newtons
  17. Landing_Thrust = 30
  18. g = -9.81       # Gravitational constant
  19. V_i = 0         #initial velocity
  20. Y_i = 0         #initial height
  21. # ------------------
  22.  
  23. # PID values
  24. # Fine tuning the PID gains
  25. # Zieglar Nicholas Method, the heuristic apporoach for fine tuning
  26. # ku = 0.5
  27. # Tu = 4 ms=iliseconds  0.0444
  28. # kp = 0.6*ku = 0.30
  29. # ki = (1.2 ku)/Tu(in seconds) = ((1.2*0.7)/4)*1000 = 13.513
  30. # kd = (3*ku*Tu)/40 = (3*0.7*(4/1000))/40 = 0.001665
  31. KP = 0.5
  32. KI = 13.513
  33. KD = 0.001665
  34.  
  35.  
  36. class Simulation(object):
  37.     def __init__(self):
  38.         self.Insight = Rocket()
  39.         self.pid = PID(KP, KI, KD, SETPOINT)
  40.         self.screen = turtle.Screen()
  41.         self.screen.setup(800, 600)
  42.         self.marker = turtle.Turtle()
  43.         self.marker.penup()
  44.         self.marker.left(180)
  45.         self.marker.goto(15, SETPOINT)
  46.         self.marker.color("red")
  47.         self.sim = True
  48.         self.timer = 0
  49.         self.poses = np.array([])
  50.         self.times = np.array([])
  51.        
  52.      
  53.  
  54.     def cycle(self):
  55.      
  56.         while(self.sim):
  57.  
  58.             thrust = self.pid.compute(self.Insight.get_y())
  59.             #print(thrust)
  60.             #print(self.target)
  61.             self.Insight.set_ddy(thrust)
  62.             self.Insight.set_dy()
  63.             self.Insight.set_y()
  64.             time.sleep(TIME_STEP)
  65.             self.timer += TIME_STEP
  66.             if self.timer > SIM_TIME:
  67.                 self.sim = False
  68.                 print("Time limit reached")
  69.  
  70.                
  71.             elif self.Insight.get_y() > 800:
  72.                 self.sim = False
  73.                 print("Upper ceiling hit")
  74.             elif self.Insight.get_y() < -800:
  75.                 self.sim = False
  76.                 print("Lower ceiling hit")
  77.  
  78.             #print(self.Insight.get_y(), self.target)
  79.            
  80.             if 100 <= self.Insight.get_y() <= 102:
  81.                 #print(" Target achieved, returning to base")
  82.                 self.pid.setpoint = 20
  83.                 self.pid.integral_error = 0 # integral accumulates all the values throught the run, reset it
  84.                 self.pid.error_last = self.pid.error # reset error, as it still has the last value from setpoint 100
  85.                 #print(SETPOINT)
  86.  
  87.             self.poses = np.append(self.poses, self.Insight.get_y())
  88.             self.times = np.append(self.times, self.timer)
  89.             #print(self.Insight.get_y)
  90.  
  91.            
  92.  
  93.         graph(self.times, self.poses)
  94.            
  95. def graph(x, y):
  96.     plt.plot(x,y)
  97.     plt.show()
  98.  
  99. class Rocket(object):
  100.     def __init__(self):
  101.         global Rocket
  102.         self.Rocket = turtle.Turtle()
  103.         self.Rocket.shape('square')
  104.         self.Rocket.color('black')
  105.         self.Rocket.penup()
  106.         self.Rocket.goto(INITIAL_X, INITIAL_Y)
  107.         self.Rocket.speed(0)
  108.  
  109.         #physics
  110.         self.ddy = 0
  111.         self.dy = V_i
  112.         self.y =    INITIAL_Y
  113.  
  114.     def set_ddy(self, thrust):
  115.         self.ddy = g + thrust / MASS
  116.     def get_ddy(self):
  117.         return self.ddy
  118.     def set_dy(self):
  119.         self.dy += self.ddy * TIME_STEP
  120.     def get_dy(self):
  121.         return self.dy
  122.     def set_y(self):
  123.         self.y += self.dy
  124.         self.Rocket.sety(self.y)
  125.     def get_y(self):
  126.         self.y = self.Rocket.ycor()
  127.         return self.y
  128.  
  129.  
  130. class PID(object):
  131.     def __init__(self, KP, KI, KD, target):
  132.         self.kp = KP
  133.         self.ki = KI
  134.         self.kd = KD
  135.         self.setpoint = target
  136.         self.error = 0
  137.         self.integral_error = 0
  138.         self.error_last = 0
  139.         self.derivative_error = 0
  140.         self.output = 0
  141.  
  142.     def compute(self, pos):
  143.         self.error = self.setpoint - pos      # error = target height - current position height
  144.         self.integral_error += self.error * TIME_STEP   # adding error wrt to time
  145.         self.derivative_error = (self.error - self.error_last) / TIME_STEP
  146.         self.error_last = self.error
  147.         self.output = self.kp*self.error + self.ki*self.integral_error + self.kd*self.derivative_error
  148.         #print(self.setpoint)
  149.         #print(self.integral_error)
  150.  
  151.         if self.output >= MAX_THRUST:
  152.             if self.setpoint == 20 and pos <= 50:
  153.                 self.output = Landing_Thrust
  154.                 if 19 <= pos <= 21:
  155.                     self.output = MAX_THRUST
  156.                    
  157.                    
  158.             else:             # Saturation responses, setting the value of the thurst to the maximum physically possible value
  159.                 self.output = MAX_THRUST         # If the PID calculates the thurst to be a value which is physically impossible to have
  160.  
  161.             if self.setpoint == 20 and pos > 21:
  162.                 self.output = -10
  163.  
  164.         elif self.output <= 0:
  165.             self.output = 0
  166.         return self.output
  167.  
  168.  
  169. def main():                         # 1 DOF PID contoller
  170.     sim = Simulation()
  171.     sim.cycle()
  172.     #turtle.done()
  173.  
  174. main()
  175.  
  176.  
  177.  
  178.  
  179.  
Advertisement
Add Comment
Please, Sign In to add comment