Source Code for Module simulation.Pendulum

  1  # -*- coding: utf-8 -*- 
  2   
  3  """ 
  4   Realizes the simulation of a pendulum. 
  5   
  6   Calculates the simulation in steps of about 10ms. 
  7   (So we don't need the correct differential equations.) 
  8   
  9   Beside the movement is also tries to model static and slipping friction. 
 10   Both values are set to the same value and simply modeled as force against  
 11   the movement direction. If the friction force is larger then the driving  
 12   force (external or own mass in movement) the movement stops. 
 13   
 14   Also contains some modifiable parameters. 
 15  """ 
 16  import math 
 17  import random 
 18   
 19   
 20 -class Pendulum(object): 
 21      """Simulation of a pendulum.""" 
 22   
 23      #: lower limit of velocity. Smaller values of velocity are considered as no movement. 
 24      NO_MOVEMENT = 0.00001 
 25   
 26 -    def __init__(self): 
 27          """Initialize the simulation.""" 
 28          self.X = 0.0  #: position [m] 
 29          self.dX_dT = 0.0 #: velocity [m/s] 
 30          self.Phi = math.radians(270.0) #: angle [rad] 
 31          self.dPhi_dT = 0.0 #: angle velocity [rad/s] 
 32   
 33          self.a = 0.0 #: acceleration [m/s²] 
 34   
 35          self.l = 1.0 #: length of pendulum [m] 
 36          self.m = 1.0 #: mass of pendulum [kg] 
 37          self.M_P = 0.1 #: friction of bearing of pendulum expressed as torque [kgm²/s²=Nm] 
 38          self.a_W = 0.1 #: friction of car expressed as acceleration [m/s²] 
 39   
 40          self.W = 1.0 #: gain for incoming acceleration value 
 41          self.Z = 0.01 #: disturbance 
 42   
 43          self.pendulum_moves = 0 
 44          self.car_moves = 0 
 45   
 46   
 47 -    def doStep(self,dT): 
 48          """Do one step of time dT. 
 49            If dT is too large it is subdivided in smaller steps.""" 
 50          n = 1 
 51          dT_n = dT 
 52   
 53          # dT is larger than 0.02 ? 
 54          if dT/0.01 >= 2.0: 
 55              # divide dT into smaller steps 
 56              n = round(dT/0.01) 
 57              dT_n = dT/float(n) 
 58   
 59          for _ in range(n): # calculate steps 
 60              self.doSmallStep(dT_n) 
 61   
 62 -    def doSmallStep(self,dT): 
 63          """Simulate a small step of not more than 0.02s.""" 
 64   
 65          while dT > 0.0: 
 66              time_left = 0.0 
 67   
 68              a = self.a * self.W # calculate used acceleration 
 69   
 70              # XXX wiederanlauf 
 71              if (self.car_moves==0) and (abs(a) > self.a_W): 
 72                  self.car_moves = 1 
 73                  if a > 0: self.dX_dT = +Pendulum.NO_MOVEMENT 
 74                  else:     self.dX_dT = -Pendulum.NO_MOVEMENT 
 75   
 76              # XXX Reibung zu gross => Stop 
 77              if (abs(self.dX_dT) < Pendulum.NO_MOVEMENT) and (abs(a) <= self.a_W): 
 78                  self.car_moves = 0 
 79                  self.dX_dT = 0.0 
 80                  a = 0.0 
 81   
 82              # XXX Wirkungsrichtung Reibung ? 
 83              if self.car_moves == 1: 
 84                  if self.dX_dT > 0: a = a - self.a_W 
 85                  else:              a = a + self.a_W 
 86   
 87                  if self.dX_dT * (self.dX_dT + a * dT) < 0.0: # XXX Nulldurchgang ? 
 88                      # Zeiteinheit teilen 
 89                      dT_ = - self.dX_dT/a 
 90                      if abs(dT_) > 1.0E-5 and abs(dT-dT_) > 1.0E-5:  
 91                          time_left = dT - dT_ # XXX Restzeit 
 92                          dT = dT_ 
 93   
 94              while dT > 0.0: 
 95                  time_left2 = 0.0 
 96   
 97                  M_D = self.l/2.0 * self.m * ( 
 98                          9.81 * -math.cos(self.Phi) # momentum of gravity 
 99                        + a *  math.sin(self.Phi) # momentum of car movement 
100                        + (self.Z * random.gauss(0.0,1.0)) * math.sin(self.Phi) # disturbance 
101                        ) 
102   
103                  if (self.pendulum_moves == 0) and (abs(M_D) > self.M_P): 
104                      # XXX Wiederanlauf 
105                      self.pendulum_moves = 1 
106                      if M_D > 0.0: self.dPhi_dT = +Pendulum.NO_MOVEMENT 
107                      else:       self.dPhi_dT = -Pendulum.NO_MOVEMENT 
108                  if (abs(self.dPhi_dT) < Pendulum.NO_MOVEMENT) and (abs(M_D)<=self.M_P): 
109                      # XXX Reibung zu groß => Stop 
110                      self.pendulum_moves = 0 
111                      self.dPhi_dT = 0.0 
112                      M_D = 0.0 
113   
114                  # Reibung im Lager beachten  
115                  # Reibung verrechnen 
116                  if self.pendulum_moves: 
117                      # Wirkrichtung Reibung ? 
118                      if self.dPhi_dT > 0: M_D = M_D - self.M_P 
119                      else:                M_D = M_D + self.M_P 
120   
121                      d2Phi_dT2 = M_D/(4.0/3.0 * self.m * self.l*self.l/4.0) # inertia 
122   
123                      if self.dPhi_dT * (self.dPhi_dT + d2Phi_dT2 * dT) < 0.0: # XXX Nulldurchgang ? 
124                          # Zeiteinheit teilen 
125                          dT_ = - self.dPhi_dT/d2Phi_dT2 
126                          if abs(dT_) > 1.0E-5 and abs(dT-dT_) > 1.0E-5:  
127                              time_left2 = dT - dT_ # XXX Restzeit 
128                              dT = dT_ 
129   
130                      # XXX neuer Winkel  
131                      self.Phi += self.dPhi_dT * dT + d2Phi_dT2 * dT*dT 
132                      while self.Phi < 0.0: self.Phi = self.Phi + 2.0*math.pi 
133                      while self.Phi >= 2.0*math.pi: self.Phi = self.Phi - 2.0*math.pi 
134   
135                      # XXX neue Winkelgeschwindigkeit 
136                      self.dPhi_dT += d2Phi_dT2 * dT 
137   
138                  # new position 
139                  self.X += self.dX_dT * dT + a*dT*dT 
140   
141                  # new speed 
142                  self.dX_dT += a*dT 
143   
144                  dT = time_left2 
145              dT = time_left 
146