home *** CD-ROM | disk | FTP | other *** search
/ Hackers Magazine 57 / CdHackersMagazineNr57.iso / Software / Multimedia / k3d-setup-0.7.11.0.exe / lib / site-packages / cgkit / pidcontroller.py < prev    next >
Encoding:
Python Source  |  2007-01-11  |  4.6 KB  |  137 lines
  1. # ***** BEGIN LICENSE BLOCK *****
  2. # Version: MPL 1.1/GPL 2.0/LGPL 2.1
  3. #
  4. # The contents of this file are subject to the Mozilla Public License Version
  5. # 1.1 (the "License"); you may not use this file except in compliance with
  6. # the License. You may obtain a copy of the License at
  7. # http://www.mozilla.org/MPL/
  8. #
  9. # Software distributed under the License is distributed on an "AS IS" basis,
  10. # WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License
  11. # for the specific language governing rights and limitations under the
  12. # License.
  13. #
  14. # The Original Code is the Python Computer Graphics Kit.
  15. #
  16. # The Initial Developer of the Original Code is Matthias Baas.
  17. # Portions created by the Initial Developer are Copyright (C) 2004
  18. # the Initial Developer. All Rights Reserved.
  19. #
  20. # Contributor(s):
  21. #
  22. # Alternatively, the contents of this file may be used under the terms of
  23. # either the GNU General Public License Version 2 or later (the "GPL"), or
  24. # the GNU Lesser General Public License Version 2.1 or later (the "LGPL"),
  25. # in which case the provisions of the GPL or the LGPL are applicable instead
  26. # of those above. If you wish to allow use of your version of this file only
  27. # under the terms of either the GPL or the LGPL, and not to allow others to
  28. # use your version of this file under the terms of the MPL, indicate your
  29. # decision by deleting the provisions above and replace them with the notice
  30. # and other provisions required by the GPL or the LGPL. If you do not delete
  31. # the provisions above, a recipient may use your version of this file under
  32. # the terms of any one of the MPL, the GPL or the LGPL.
  33. #
  34. # ***** END LICENSE BLOCK *****
  35. # $Id: pidcontroller.py,v 1.1.1.1 2004/12/12 14:31:11 mbaas Exp $
  36.  
  37. ## \file pidcontroller.py
  38. ## Contains the PIDController class.
  39.  
  40. import component
  41. import eventmanager, events
  42. from scene import getScene
  43. from slots import *
  44. from cgtypes import *
  45. import _core
  46.  
  47. # PIDController
  48. class PIDController(component.Component):
  49.     """PID controller.
  50.  
  51.     """
  52.  
  53.     exec slotPropertyCode("input")
  54.     exec slotPropertyCode("output")
  55.     exec slotPropertyCode("setpoint")
  56.     exec slotPropertyCode("maxout")
  57.     exec slotPropertyCode("minout")
  58.     exec slotPropertyCode("Kp")
  59.     exec slotPropertyCode("Ki")
  60.     exec slotPropertyCode("Kd")
  61.  
  62.     def __init__(self,
  63.                  name = "PIDController",
  64.                  setpoint = 0.0,
  65.                  Kp = 0.0,
  66.                  Ki = 0.0,
  67.                  Kd = 0.0,
  68.                  maxout = 999999,
  69.                  minout = -999999,
  70.                  auto_insert = True):
  71.         """Constructor.
  72.         """
  73.         
  74.         component.Component.__init__(self, name, auto_insert)
  75.  
  76.         self.input_slot = DoubleSlot()
  77.         self.setpoint_slot = DoubleSlot(setpoint)
  78.         self.maxout_slot = DoubleSlot(maxout)
  79.         self.minout_slot = DoubleSlot(minout)
  80.         self.Kp_slot = DoubleSlot(Kp)
  81.         self.Ki_slot = DoubleSlot(Ki)
  82.         self.Kd_slot = DoubleSlot(Kd)
  83.  
  84.         self.output_slot = ProceduralDoubleSlot(self.computeOutput)
  85.  
  86.         self.addSlot("input", self.input_slot)
  87.         self.addSlot("setpoint", self.setpoint_slot)
  88.         self.addSlot("output", self.output_slot)
  89.         self.addSlot("maxout", self.maxout_slot)
  90.         self.addSlot("minout", self.minout_slot)
  91.         self.addSlot("Kp", self.Kp_slot)
  92.         self.addSlot("Ki", self.Ki_slot)
  93.         self.addSlot("Kd", self.Kd_slot)
  94.  
  95.         self.input_slot.addDependent(self.output_slot)
  96.         self.setpoint_slot.addDependent(self.output_slot)
  97.         self.maxout_slot.addDependent(self.output_slot)
  98.         self.minout_slot.addDependent(self.output_slot)
  99.         self.Kp_slot.addDependent(self.output_slot)
  100.         self.Ki_slot.addDependent(self.output_slot)
  101.         self.Kd_slot.addDependent(self.output_slot)
  102.  
  103.         self._integral = 0.0
  104.         self._prev_err = 0.0
  105.  
  106.         eventmanager.eventManager().connect(events.STEP_FRAME, self)
  107.         eventmanager.eventManager().connect(events.RESET, self)
  108.         
  109.  
  110.     def onStepFrame(self):
  111.         err = self.setpoint-self.input
  112.         dt = getScene().timer().timestep
  113.         self._integral += dt*err
  114.  
  115.     def onReset(self):
  116.         self._integral = 0.0
  117.         self._prev_err = 0.0
  118.  
  119.     def computeOutput(self):
  120.         err = self.setpoint-self.input
  121.         dt = getScene().timer().timestep
  122.         I = self._integral
  123.         D = (err-self._prev_err)/dt
  124. #        print "D:",D
  125.         res = self.Kp*err + self.Ki*I + self.Kd*D
  126.         
  127.         self._prev_err = err
  128.         
  129.         maxout = self.maxout
  130.         minout = self.minout
  131.         if res>maxout:
  132.             res = maxout
  133.         elif res<minout:
  134.             res = minout
  135.             
  136.         return res
  137.