1  #!/usr/bin/env python
  2  
  3  import linuxcnc
  4  import hal
  5  import time
  6  import sys
  7  import os
  8  
  9  
 10  # this is how long we wait for linuxcnc to do our bidding
 11  timeout = 5.0
 12  
 13  
 14  # unbuffer stdout
 15  sys.stdout = os.fdopen(sys.stdout.fileno(), 'w', 0)
 16  
 17  
 18  program_start = time.time()
 19  
 20  def log(msg):
 21      delta_t = time.time() - program_start;
 22      print "%.3f: %s" % (delta_t, msg)
 23      sys.stdout.flush()
 24  
 25  
 26  def introspect(h):
 27      os.system("halcmd show pin halui")
 28      os.system("halcmd show pin python-ui")
 29      os.system("halcmd show sig")
 30  
 31  
 32  def home_joint(name):
 33      log("    homing %s" % name)
 34  
 35      h[name + '-home'] = 1
 36      start = time.time()
 37      while (h[name + '-homed'] == 0) and ((time.time() - start) < timeout):
 38          time.sleep(0.1)
 39  
 40      if h[name + '-homed'] == 0:
 41          log("failed to home %s in halui" % name)
 42          introspect(h)
 43          sys.exit(1)
 44  
 45      h[name + '-home'] = 0
 46  
 47  
 48  def select_joint(name, deassert=True):
 49      log("    selecting %s" % name)
 50  
 51      h[name + '-select'] = 1
 52      start = time.time()
 53      while (h[name + '-selected'] == 0) and ((time.time() - start) < timeout):
 54          time.sleep(0.1)
 55  
 56      if h[name + '-selected'] == 0:
 57          log("failed to select %s in halui" % name)
 58          introspect(h)
 59          sys.exit(1)
 60  
 61      if deassert:
 62          h[name + '-select'] = 0
 63  
 64  
 65  def jog_minus(name, target):
 66      start_position = h[name + '-position']
 67      log("    jogging %s negative: to %.3f" % (name, target))
 68  
 69      h['jog-selected-minus'] = 1
 70  
 71      start = time.time()
 72      while (h[name + '-position'] > target) and ((time.time() - start) < timeout):
 73          time.sleep(0.1)
 74  
 75      if h[name + '-position'] > target:
 76          log("failed to jog %s to %.3f (timed out at %.3f after %.3f seconds)" % (name, target, h[name + '-position'], timeout))
 77          introspect(h)
 78          sys.exit(1)
 79  
 80      h['jog-selected-minus'] = 0
 81  
 82      log("    jogged %s negative past target %.3f" % (name, target))
 83  
 84      return True
 85  
 86  
 87  def jog_plus(name, target):
 88      start_position = h[name + '-position']
 89      log("    jogging %s positive: to %.3f" % (name, target))
 90  
 91      h['jog-selected-plus'] = 1
 92  
 93      start = time.time()
 94      while (h[name + '-position'] < target) and ((time.time() - start) < timeout):
 95          time.sleep(0.1)
 96  
 97      if h[name + '-position'] < target:
 98          log("failed to jog %s to %.3f (timed out at %.3f after %.3f seconds)" % (name, target, h[name + '-position'], timeout))
 99          introspect(h)
100          sys.exit(1)
101  
102      h['jog-selected-plus'] = 0
103  
104      log("    jogged %s positive past target %.3f" % (name, target))
105  
106      return True
107  
108  
109  def wait_for_pin_value(pin_name, target_value, timeout=2.0):
110      start_time = time.time()
111      while h[pin_name] != target_value:
112          if (time.time() - start_time) > timeout:
113              log("Error: pin %s didn't reach target value %s!" % (pin_name, target_value))
114              log("pin value is %s at timeout after %.3f seconds" % (h[pin_value], timeout))
115              sys.exit(1)
116          time.sleep(0.1)
117  
118  
119  def wait_for_joint_to_stop(joint_number):
120      pos_pin = 'joint-%d-position' % joint_number
121      vel_pin = 'joint-%d-velocity' % joint_number
122      start_time = time.time()
123      prev_pos = h[pos_pin]
124      while (time.time() - start_time) < timeout:
125          time.sleep(0.1)
126          new_pos = h[pos_pin]
127          if new_pos == prev_pos and h[vel_pin] == 0.0:
128              log("joint %d stopped jogging" % joint_number)
129              return
130          prev_pos = new_pos
131      log("Error: joint didn't stop jogging!")
132      log("joint %d is at %.3f %.3f seconds after reaching target (prev_pos=%.3f, val=%.3f)" % (joint_number, h[pos_pin], timeout, prev_pos, h[vel_pin]))
133      sys.exit(1)
134  
135  
136  def jog_joint(joint_number, target):
137      success = True
138  
139      joint = []
140      for j in range(0,3):
141          joint.append(h['joint-%d-position' % j])
142  
143      name = 'joint-%d' % joint_number
144  
145      log("jogging %s to %.3f" % (name, target))
146      select_joint(name)
147  
148      if h[name + '-position'] > target:
149          jog_minus(name, target)
150      else:
151          jog_plus(name, target)
152  
153      for j in range(0,3):
154          pin_name = 'joint-%d-position' % j
155          if j == joint_number:
156              if joint[j] == h[pin_name]:
157                  log("joint %d didn't move but should have!" % j)
158                  success = False
159          else:
160              if joint[j] != h[pin_name]:
161                  log("joint %d moved from %.3f to %.3f but shouldnt have!" % (j, joint[j], h[pin_name]))
162                  success = False
163  
164      wait_for_joint_to_stop(joint_number)
165  
166      if not success:
167          sys.exit(1)
168  
169  
170  #
171  # set up pins
172  # shell out to halcmd to make nets to halui and motion
173  #
174  
175  h = hal.component("python-ui")
176  
177  h.newpin("joint-0-home", hal.HAL_BIT, hal.HAL_OUT)
178  h.newpin("joint-0-homed", hal.HAL_BIT, hal.HAL_IN)
179  h.newpin("joint-0-select", hal.HAL_BIT, hal.HAL_OUT)
180  h.newpin("joint-0-selected", hal.HAL_BIT, hal.HAL_IN)
181  h.newpin("joint-0-position", hal.HAL_FLOAT, hal.HAL_IN)
182  h.newpin("joint-0-velocity", hal.HAL_FLOAT, hal.HAL_IN)
183  h.newpin("joint-0-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
184  h.newpin("joint-0-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
185  
186  h.newpin("joint-1-home", hal.HAL_BIT, hal.HAL_OUT)
187  h.newpin("joint-1-homed", hal.HAL_BIT, hal.HAL_IN)
188  h.newpin("joint-1-select", hal.HAL_BIT, hal.HAL_OUT)
189  h.newpin("joint-1-selected", hal.HAL_BIT, hal.HAL_IN)
190  h.newpin("joint-1-position", hal.HAL_FLOAT, hal.HAL_IN)
191  h.newpin("joint-1-velocity", hal.HAL_FLOAT, hal.HAL_IN)
192  h.newpin("joint-1-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
193  h.newpin("joint-1-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
194  
195  h.newpin("joint-2-home", hal.HAL_BIT, hal.HAL_OUT)
196  h.newpin("joint-2-homed", hal.HAL_BIT, hal.HAL_IN)
197  h.newpin("joint-2-select", hal.HAL_BIT, hal.HAL_OUT)
198  h.newpin("joint-2-selected", hal.HAL_BIT, hal.HAL_IN)
199  h.newpin("joint-2-position", hal.HAL_FLOAT, hal.HAL_IN)
200  h.newpin("joint-2-velocity", hal.HAL_FLOAT, hal.HAL_IN)
201  h.newpin("joint-2-jog-plus", hal.HAL_BIT, hal.HAL_OUT)
202  h.newpin("joint-2-jog-minus", hal.HAL_BIT, hal.HAL_OUT)
203  
204  h.newpin("jog-selected-minus", hal.HAL_BIT, hal.HAL_OUT)
205  h.newpin("jog-selected-plus", hal.HAL_BIT, hal.HAL_OUT)
206  
207  h.newpin("motion-mode-joint", hal.HAL_BIT, hal.HAL_OUT)
208  h.newpin("motion-mode-is-joint", hal.HAL_BIT, hal.HAL_IN)
209  
210  h.ready() # mark the component as 'ready'
211  
212  os.system("halcmd source ./postgui.hal")
213  
214  
215  #
216  # connect to LinuxCNC
217  #
218  
219  c = linuxcnc.command()
220  s = linuxcnc.stat()
221  e = linuxcnc.error_channel
222  
223  c.state(linuxcnc.STATE_ESTOP_RESET)
224  c.state(linuxcnc.STATE_ON)
225  c.wait_complete()
226  
227  # Select joints 1 and 2, but do not de-assert the .select pins.
228  select_joint('joint-1', deassert=False)
229  select_joint('joint-2', deassert=False)
230  
231  # Home all the joints.  This should work fine.
232  home_joint('joint-0')
233  home_joint('joint-1')
234  home_joint('joint-2')
235  
236  # Deassert the .select pins for joints 1 and 2.
237  h['joint-1-select'] = 0
238  h['joint-2-select'] = 0
239  
240  # The machine is homed and all the .joint.N.select pins are deasserted.
241  c.mode(linuxcnc.MODE_MANUAL)
242  c.wait_complete()
243  
244  h['motion-mode-joint'] = 1
245  wait_for_pin_value('motion-mode-is-joint', 1, 2.0)
246  h['motion-mode-joint'] = 0
247  
248  
249  #
250  # First some simple single-axis jog & stop.  Test each axis jogging in each
251  # direction, one at a time.
252  #
253  # These jog_joint() functions will exit with a return value of 1 if
254  # something goes wrong, signalling test failure.
255  #
256  
257  jog_joint(0, -0.5)
258  jog_joint(0, 0.0)
259  
260  jog_joint(1, -0.5)
261  jog_joint(1, 0.0)
262  
263  jog_joint(2, -0.5)
264  jog_joint(2, 0.0)
265  
266  
267  #
268  # Next try selecting a joint and jogging it with the "jog selected" pins,
269  # then changing which joint is selected.  The expected behavior when
270  # changing which joint is selected is that the old joint stops jogging and
271  # the new one starts jogging.
272  #
273  # We do this while jogging yet a third joint using its private, per-joint
274  # jog pins, to verify that it is unaffected by all the drama.
275  #
276  
277  name0 = 'joint-0'
278  name1 = 'joint-1'
279  name2 = 'joint-2'
280  
281  start_position0 = h[name0 + '-position']
282  start_position1 = h[name1 + '-position']
283  start_position2 = h[name2 + '-position']
284  
285  print "%s starting at %.3f" % (name0, start_position0)
286  print "%s starting at %.3f" % (name1, start_position1)
287  print "%s starting at %.3f" % (name2, start_position2)
288  
289  
290  #
291  # start joint0 jogging in the positive direction
292  #
293  
294  print "jogging %s positive using the private per-joint jog pins" % name0
295  h[name0 + '-jog-plus'] = 1
296  
297  # wait for this joint to come up to speed
298  start = time.time()
299  while (h[name0 + '-velocity'] <= 0) and ((time.time() - start) < timeout):
300      time.sleep(0.1)
301  if h[name0 + '-velocity'] <= 0:
302      print "%s did not start jogging" % name0
303      sys.exit(1)
304  
305  
306  #
307  # start the selected joint1 jogging in the negative direction
308  #
309  
310  print "jogging selected joint (%s) negative" % (name1)
311  select_joint(name1)
312  h['jog-selected-minus'] = 1
313  
314  # wait for this joint to start moving
315  start = time.time()
316  while (h[name1 + '-velocity'] >= 0) and ((time.time() - start) < timeout):
317      time.sleep(0.1)
318  if h[name1 + '-velocity'] >= 0:
319      print "%s did not start jogging" % name1
320      sys.exit(1)
321  
322  if h[name0 + '-velocity'] <= 0:
323      print "%s stopped jogging" % name0
324      sys.exit(1)
325  
326  if h[name1 + '-position'] >= start_position1:
327      print "%s was selected but did not jog negative (start=%.3f, end=%.3f)" % (name1, start_position1, h[name1 + '-position'])
328      sys.exit(1)
329  
330  if h[name2 + '-position'] != start_position2:
331      print "%s was not selected but moved (start=%.3f, end=%.3f)" % (name2, start_position2, h[name2 + '-position'])
332      sys.exit(1)
333  
334  print "%s was selected and jogged, %s was not selected and stayed still" % (name1, name2)
335  
336  
337  start_position1 = h[name1 + '-position']
338  start_position2 = h[name2 + '-position']
339  
340  start_velocity1 = h[name1 + '-velocity']
341  start_velocity2 = h[name2 + '-velocity']
342  
343  print "selecting %s" % name2
344  select_joint(name2)
345  
346  wait_for_joint_to_stop(1)
347  
348  if h[name0 + '-velocity'] <= 0:
349      print "%s stopped jogging" % name0
350      sys.exit(1)
351  
352  if h[name1 + '-velocity'] != 0:
353      print "%s was deselected but did not stop (start_vel=%.3f, end_vel=%.3f)" % (name1, start_velocity1, h[name1 + '-velocity'])
354      sys.exit(1)
355  
356  if h[name2 + '-velocity'] >= 0:
357      print "%s was selected but did not move (start_vel=%.3f, end_vel=%.3f)" % (name2, start_velocity2, h[name2 + '-velocity'])
358      sys.exit(1)
359  
360  print "%s was deselected and stopped, %s was selected and jogged" % (name1, name2)
361  
362  
363  start_velocity1 = h[name1 + '-velocity']
364  start_velocity2 = h[name2 + '-velocity']
365  
366  print "stopping jog"
367  h['jog-selected-minus'] = 0
368  
369  wait_for_joint_to_stop(2)
370  
371  if h[name0 + '-velocity'] <= 0:
372      print "%s stopped jogging" % name0
373      sys.exit(1)
374  
375  if h[name1 + '-velocity'] != 0:
376      print "%s started moving again (start_vel=%.3f, end_vel=%.3f)" % (name1, start_velocity1, h[name1 + '-velocity'])
377      sys.exit(1)
378  
379  if h[name2 + '-velocity'] != 0:
380      print "%s did not stop (start_vel=%.3f, end_vel=%.3f)" % (name2, start_velocity2, h[name2 + '-velocity'])
381      sys.exit(1)
382  
383  print "%s stopped" % name2
384  
385  
386  sys.exit(0)
387