Guest User

touchdown

a guest
Dec 15th, 2011
969
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