SHARE
TWEET

touchdown

a guest Dec 15th, 2011 781 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/python
  2.  
  3. import pyaudio
  4. import numpy
  5. import sys
  6. import struct
  7. import hal
  8. import time
  9.  
  10. #Functions
  11.  
  12.  
  13. #HAL Stuff
  14.  
  15. h = hal.component("tddetect")
  16. h.newpin("enable", hal.HAL_BIT, hal.HAL_IO)
  17. h.newpin("is_mdi_mode", hal.HAL_BIT, hal.HAL_IN)
  18. h.newpin("spindle_speed", hal.HAL_FLOAT, hal.HAL_IN)
  19. h.newpin("touch", hal.HAL_BIT, hal.HAL_OUT)
  20. h.newparam("mov_avg", hal.HAL_FLOAT, hal.HAL_RO)
  21. h.newparam("pct_change", hal.HAL_FLOAT, hal.HAL_RO)
  22. h.newparam("pbc", hal.HAL_U32, hal.HAL_RO)
  23. h.newparam("frange", hal.HAL_U32, hal.HAL_RW)
  24. h.newparam("mov_avg_window", hal.HAL_U32, hal.HAL_RW)
  25. h.newparam("td_thresh", hal.HAL_FLOAT, hal.HAL_RW)
  26. h.ready()
  27.  
  28. h['mov_avg_window'] = 1
  29.  
  30. float_buffer   = numpy.zeros(1024)
  31. mov_avg_buffer = numpy.zeros(1024)
  32. mov_avg_index  = 0
  33. power_base_acc = 0.0
  34. power_base     = 0.0
  35. power_base_cnt = 0
  36. initialize = True
  37. chk_enable = False
  38.  
  39. chunk = 1024
  40. FORMAT = pyaudio.paFloat32
  41. CHANNELS = 1
  42. RATE = 44100
  43. F_STEP = 44100.0 / chunk  
  44.  
  45. p = pyaudio.PyAudio()
  46.  
  47. stream = p.open(format = FORMAT,
  48.                 channels = CHANNELS,
  49.                 rate = RATE,
  50.                 input = True,
  51.                 frames_per_buffer = chunk)
  52.  
  53. debug = True
  54.  
  55. try:
  56.         while 1:
  57.                 if (h['enable'] and h['is_mdi_mode']):
  58.                         lfreq = (h['spindle_speed'] / 60.0) - (h['frange'] / 2)
  59.                         lfreq_bin = round(lfreq / F_STEP)                      
  60.                         hfreq = lfreq + (2 * h['frange'])
  61.                         hfreq_bin = round(hfreq / F_STEP)
  62. #                       if debug:
  63. #                               print "lfreq_bin:", lfreq_bin, " hfreq_bin", hfreq_bin
  64.  
  65.                         data = stream.read(chunk)
  66.                         float_buffer = struct.unpack('@' + 'f'*1024, data)
  67.                         b = numpy.array(float_buffer)
  68.                         c = numpy.fft.rfft(b)
  69.  
  70.                         d = numpy.square(abs(c))
  71.                         tot_pow = numpy.sum(d[lfreq_bin:hfreq_bin+1])
  72.                         mov_avg_buffer[mov_avg_index] = tot_pow
  73. #                       if debug:
  74. #                               print "mov_avg_buf: ", mov_avg_buffer[0:h['mov_avg_window']]
  75.  
  76.                         h['pbc'] = power_base_cnt
  77.  
  78.                         if power_base_cnt < h['mov_avg_window']:
  79.                                 power_base_acc += tot_pow
  80.                                 power_base = power_base_acc / h['mov_avg_window']
  81.                                 power_base_cnt += 1
  82. #                               print power_base_acc, ' ', power_base
  83.                                 chk_enable = False
  84.                         else:
  85.                                 chk_enable = True
  86.  
  87.                         mov_avg = numpy.sum(mov_avg_buffer[0:h['mov_avg_window']]) / h['mov_avg_window']
  88. #                       if debug or not chk_enable :
  89. #                               print 'mov_avg: ', mov_avg
  90.  
  91.                         mov_avg_index += 1
  92.                         if mov_avg_index == h['mov_avg_window']:
  93.                                 mov_avg_index = 0
  94.  
  95.                         h['mov_avg'] = float(mov_avg)
  96.  
  97.                         pct_change = abs((mov_avg - power_base)/power_base) * 100
  98.  
  99.                         h['pct_change'] = float(pct_change)
  100.  
  101.                         initialize = True
  102.  
  103.                         if ((pct_change > h['td_thresh']) and chk_enable)  :
  104. #                               print "pct_change: ", pct_change, ' chk_enable: ', chk_enable
  105.                                 h['touch'] = True
  106.                                 time.sleep(0.1)
  107.                                 h['enable'] = False
  108. #                               print "Detected Power Change"
  109.  
  110.                         debug = False
  111.                 else:
  112.                         if initialize :
  113.                                 h['touch'] = False
  114. #                               if debug:
  115. #                                       print "Initializing"
  116.                                 float_buffer   = numpy.zeros(1024)
  117.                                 mov_avg_buffer = numpy.zeros(1024)
  118.                                 mov_avg_index  = 0
  119.                                 power_base_acc = 0.0
  120.                                 power_base     = 0.0
  121.                                 power_base_cnt = 0
  122.                                 initialize = False
  123.                                 chk_enable = False
  124.                                 debug = True
  125.        
  126.  
  127. except  KeyboardInterrupt:
  128.         raise SystemExit
  129. finally:
  130.         stream.close()
  131.         p.terminate()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Top