adding files

my-emco/luber.py

@@ -0,0 +1,234 @@
+#! /usr/bin/python
+
+import sys
+import getopt
+import xml.etree.ElementTree as ET
+import hal
+import time
+from collections import namedtuple
+
+class Pin:
+   """ Representation of a Pin and it's data"""
+   def __init__(self, type, dir):
+      self.val = 0    # current value of pin, e.g. 1 - on, 0 - off
+      self.type = type  # type (string read from xml)
+      self.dir = dir
+
+   def __repr__(self):
+      return 'val: ' + str(self.val) + '\ttype: ' + self.type + '\tdir: ' + self.dir
+
+class HalAdapter:
+   def __init__(self, name):
+      self.h = hal.component(name) 
+      self.h.newpin("velocity", hal.HAL_FLOAT, hal.HAL_OUT)
+      self.h.newpin('x-vel', hal.HAL_FLOAT, hal.HAL_IN)
+      self.h.newpin('y-vel', hal.HAL_FLOAT, hal.HAL_IN)
+      self.h.newpin('z-vel', hal.HAL_FLOAT, hal.HAL_IN)
+      self.h.newpin('lube-level-ok',hal.HAL_BIT, hal.HAL_IN)
+      self.h.newpin('reset', hal.HAL_BIT, hal.HAL_IN)
+      self.h.newpin('lube-ext-req', hal.HAL_BIT, hal.HAL_IN)
+      self.h.newpin('lube-cmd', hal.HAL_BIT, hal.HAL_OUT)
+      self.h.newpin('lube-level-alarm', hal.HAL_BIT, hal.HAL_OUT)
+      self.h.newpin('accumulated-distance', hal.HAL_FLOAT, hal.HAL_OUT)
+      self.h.ready()
+
+   def __repr__(self):
+      tmp_str = ''
+      return tmp_str
+
+   def is_lube_level_ok(self):
+      return self.h['lube-level-ok']
+
+   def is_reset(self):
+      return self.h['reset']
+
+   def is_lube_ext_req(self):
+      return self.h['lube-ext-req']
+
+   def get_velocities(self):
+      velocities = namedtuple("velocities", ["x", "y", "z"])
+      return velocities(
+         self.h['x-vel'],
+         self.h['y-vel'],
+         self.h['z-vel'])
+
+   def set_lube_on(self,  request):
+      if request >= 1:
+         self.h['lube-cmd'] = 1
+      else:
+         self.h['lube-cmd'] = 0
+
+   def set_lube_level_alarm(self, level_ok):
+      if level_ok >= 1:
+         self.h['lube-level-alarm'] = 1
+      else:
+         self.h['lube-level-alarm'] = 0
+
+   def set_accumulated_distance(self, d):
+      self.h['accumulated-distance'] = d
+
+class parameterContainer:
+   def __init__(self, xml_file):
+      self.paramDict = {}
+      self._xmlFile = xml_file
+
+      self.tree = ET.parse(self._xmlFile)
+      self._parse()
+
+   def _parse(self):
+      root = self.tree.getroot()
+      for param in root.iter('parameter'):
+         #print param.attrib['name'], param.attrib['value'] 
+         self.paramDict[param.attrib['name']] = float(param.attrib['value'])
+
+   def getParam(self, name):
+      if name in self.paramDict:
+         return self.paramDict[name]
+      else:
+         return None 
+
+   def getParams(self):
+      return self.paramDict
+
+   def writeToFile(self):
+      for parName in self.paramDict:
+         self._writeToTree(parName, self.paramDict[parName])
+
+      self.tree.write(self._xmlFile)
+
+   def writeParam(self, parName, value):
+      if parName in self.paramDict:
+         self.paramDict[parName] = value
+
+   def _writeToTree(self, parName, value):   
+      """update parameter in xml-tree"""
+      root = self.tree.getroot()
+      
+      for param in root.iter('parameter'):
+           if param.attrib['name'] == parName:
+               param.attrib['value'] = str(round(value, 2))
+               break
+
+
+class LubeControl:
+   def __init__(self, lube_on_time, accumulated_distance, distance_threshold, number_of_lubings):
+      self.lubeOnTime = lube_on_time                   # [sec]
+      self.total_distance = accumulated_distance       # [mm]
+      self.distance_threshold = distance_threshold     # [mm]
+      self.numberOfLubings = number_of_lubings
+
+      self.state = 'OFF'
+      self.lubeLevelOkOut = True
+      self._lubeLevelOkIn = True
+      self.prev_time = time.time()
+
+   def calc_dist_from_vel(self, v_x, v_y, v_z):
+      current_time = time.time()
+      time_delta = current_time - self.prev_time
+
+      self.total_distance += abs(v_x) * time_delta
+      self.total_distance += abs(v_y) * time_delta
+      self.total_distance += abs(v_z) * time_delta
+
+      self.prev_time = current_time
+
+
+   def runStateMachine(self, ext_req):
+      currentTime = time.time()
+      if self.total_distance >= self.distance_threshold or ext_req == True:
+         self.state = 'ON'
+         self.timeout = self.lubeOnTime + currentTime
+         self.total_distance = 0
+         self.numberOfLubings += 1
+
+      if self.state == 'ON':
+         if currentTime > self.timeout:
+            #check lube pressure sensor
+            self.lubeLevelOkOut = self._lubeLevelOkIn
+
+            self.state = 'OFF'
+               
+
+   def setLubeLevelOK(self, levelOk):
+      self._lubeLevelOkIn = levelOk
+
+   def reset(self):
+      self.total_distance = 0
+      self.state = 'OFF'
+      self.lubeLevelOkOut = True
+
+def _usage():
+   """ print command line options """
+   print "usage luber.py -h -c<name> <path/>in_file.xml\n"\
+      "in_file         # input xml-file describing what knobs and/or button are on the pendant\n"\
+      "-c <name>       # name of component in HAL. 'my-luber' default\n"\
+      "-h              # Help test"
+
+def main():
+   xmlFile = 'luber.xml'
+   #xmlFile = ''
+   name = 'my-luber'       # default name of component in HAL
+
+   try:
+        opts, args = getopt.getopt(sys.argv[1:], "hc:", ["input="])
+   except getopt.GetoptError as err:
+      # print help information and exit:
+      print(err) # will print something like "option -a not recognized"
+      sys.exit(2)
+
+   for o, a in opts:
+      if o == "-h":
+         _usage()
+         sys.exit()
+      elif o == "-c":
+         name = a
+      elif o == "--input":
+         xmlFile = a
+      else:
+         print o
+         assert False, "unhandled option"
+
+   if xmlFile == '':
+      if len(sys.argv) < 2:
+         _usage()
+         sys.exit(2)
+      else:
+         xmlFile = sys.argv[-1]
+            
+   p = parameterContainer(xmlFile)
+
+   h = HalAdapter(name) 
+   
+   totalDistance = p.getParam('totalDistance')
+   distanceThreshold = p.getParam('distanceThreshold')
+   lubeOnTime = p.getParam('lubePulseTime')
+   nbrOfLubings = p.getParam('numberOfLubings')
+
+   lubeCtrl = LubeControl(lubeOnTime, totalDistance, distanceThreshold, nbrOfLubings)
+
+   try:
+      while 1:
+         if h.is_reset():
+            lubeCtrl.reset()
+
+         lubeCtrl.setLubeLevelOK(h.is_lube_level_ok())
+         v = h.get_velocities()
+         lubing_external_request = h.is_lube_ext_req()
+         lubeCtrl.calc_dist_from_vel(v.x, v.y, v.z)
+
+         lubeCtrl.runStateMachine(lubing_external_request)
+
+         h.set_lube_on(lubeCtrl.state == 'ON')
+         h.set_lube_level_alarm(lubeCtrl.lubeLevelOkOut)
+         h.set_accumulated_distance(lubeCtrl.total_distance)
+
+         time.sleep(0.1)
+
+   except KeyboardInterrupt:
+      p.writeParam('totalDistance', lubeCtrl.total_distance)
+      p.writeParam('numberOfLubings', lubeCtrl.numberOfLubings)
+      p.writeToFile()
+      raise SystemExit
+
+if __name__ == '__main__':
+   main()