1  #!/usr/bin/env python
  2  
  3  import linuxcnc
  4  import hal
  5  import time
  6  import sys
  7  import os
  8  import math
  9  
 10  
 11  # this is how long we wait for linuxcnc to do our bidding
 12  timeout = 1.0
 13  
 14  
 15  program_start = time.time()
 16  
 17  def log(msg):
 18      delta_t = time.time() - program_start;
 19      print "%.3f: %s" % (delta_t, msg)
 20      sys.stdout.flush()
 21  
 22  
 23  def introspect():
 24      os.system("halcmd show pin halui")
 25      os.system("halcmd show pin python-ui")
 26      os.system("halcmd show sig")
 27  
 28  
 29  def wait_for_joint_to_stop_at(joint, target):
 30      timeout = 10.0
 31      tolerance = 0.0001
 32  
 33      start = time.time()
 34  
 35      curr_pos = 0;
 36      while ((time.time() - start) < timeout):
 37          prev_pos = curr_pos
 38          curr_pos = h['joint-%d-position' % joint]
 39          vel = curr_pos - prev_pos
 40          error = math.fabs(curr_pos - target)
 41          if (error < tolerance) and (vel == 0):
 42              log("joint %d stopped at %.3f" % (joint, target))
 43              return
 44          time.sleep(0.1)
 45      log("timeout waiting for joint %d to stop at %.3f (pos=%.3f, vel=%.3f)" % (joint, target, curr_pos, vel))
 46      sys.exit(1)
 47  
 48  
 49  def wait_for_task_mode(target):
 50      timeout = 5.0
 51      start = time.time()
 52  
 53      while ((time.time() - start) < timeout):
 54          s.poll()
 55          if s.task_mode == target:
 56              return
 57          time.sleep(0.1)
 58  
 59      log("timeout waiting for task mode to get to  %d (it's %d)" % (target, s.task_mode))
 60      sys.exit(1)
 61  
 62  
 63  def wait_for_halui_mode(pin_name):
 64      timeout = 5.0
 65  
 66      start = time.time()
 67  
 68      while ((time.time() - start) < timeout):
 69          if h[pin_name]:
 70              print "halui reports mode", pin_name
 71              return
 72          time.sleep(0.1)
 73      print "timeout waiting for halui to report mode", pin_name
 74      sys.exit(1)
 75  
 76  
 77  
 78  #
 79  # set up pins
 80  # shell out to halcmd to make nets to halui and motion
 81  #
 82  
 83  h = hal.component("python-ui")
 84  
 85  h.newpin("mdi-0", hal.HAL_BIT, hal.HAL_OUT)
 86  h.newpin("mdi-1", hal.HAL_BIT, hal.HAL_OUT)
 87  h.newpin("mdi-2", hal.HAL_BIT, hal.HAL_OUT)
 88  h.newpin("mdi-3", hal.HAL_BIT, hal.HAL_OUT)
 89  
 90  h.newpin("joint-0-position", hal.HAL_FLOAT, hal.HAL_IN)
 91  h.newpin("joint-1-position", hal.HAL_FLOAT, hal.HAL_IN)
 92  h.newpin("joint-2-position", hal.HAL_FLOAT, hal.HAL_IN)
 93  
 94  h.newpin("is-manual", hal.HAL_BIT, hal.HAL_IN)
 95  h.newpin("is-auto", hal.HAL_BIT, hal.HAL_IN)
 96  h.newpin("is-mdi", hal.HAL_BIT, hal.HAL_IN)
 97  
 98  h.ready() # mark the component as 'ready'
 99  
100  os.system("halcmd source ./postgui.hal")
101  
102  
103  #
104  # connect to LinuxCNC
105  #
106  
107  c = linuxcnc.command()
108  s = linuxcnc.stat()
109  e = linuxcnc.error_channel()
110  
111  c.state(linuxcnc.STATE_ESTOP_RESET)
112  c.state(linuxcnc.STATE_ON)
113  c.wait_complete()
114  
115  
116  #
117  # run the test
118  #
119  # These functions will exit with a return value of 1 if something goes
120  # wrong.
121  #
122  
123  log("setting mode to Manual")
124  c.mode(linuxcnc.MODE_MANUAL)
125  wait_for_halui_mode('is-manual')
126  log("running MDI command 0")
127  h['mdi-0'] = 1
128  wait_for_joint_to_stop_at(0, -1);
129  wait_for_joint_to_stop_at(1, 0);
130  wait_for_joint_to_stop_at(2, 0);
131  h['mdi-0'] = 0
132  wait_for_task_mode(linuxcnc.MODE_MANUAL)
133  wait_for_halui_mode('is-manual')
134  
135  log("setting mode to Auto")
136  c.mode(linuxcnc.MODE_AUTO)
137  wait_for_halui_mode('is-auto')
138  log("running MDI command 1")
139  h['mdi-1'] = 1
140  wait_for_joint_to_stop_at(0, 1);
141  wait_for_joint_to_stop_at(1, 0);
142  wait_for_joint_to_stop_at(2, 0);
143  h['mdi-1'] = 0
144  wait_for_task_mode(linuxcnc.MODE_AUTO)
145  wait_for_halui_mode('is-auto')
146  
147  log("setting mode to MDI")
148  c.mode(linuxcnc.MODE_MDI)
149  wait_for_halui_mode('is-mdi')
150  log("running MDI command 2")
151  h['mdi-2'] = 1
152  wait_for_joint_to_stop_at(0, 1);
153  wait_for_joint_to_stop_at(1, 2);
154  wait_for_joint_to_stop_at(2, 0);
155  h['mdi-2'] = 0
156  s.poll()
157  wait_for_task_mode(linuxcnc.MODE_MDI)
158  wait_for_halui_mode('is-mdi')
159  
160  log("running MDI command 3")
161  h['mdi-3'] = 1
162  wait_for_joint_to_stop_at(0, 1);
163  wait_for_joint_to_stop_at(1, 2);
164  wait_for_joint_to_stop_at(2, 3);
165  h['mdi-3'] = 0
166  wait_for_task_mode(linuxcnc.MODE_MDI)
167  wait_for_halui_mode('is-mdi')
168  
169  log("running MDI command 0")
170  h['mdi-0'] = 1
171  wait_for_joint_to_stop_at(0, -1);
172  wait_for_joint_to_stop_at(1, 0);
173  wait_for_joint_to_stop_at(2, 0);
174  h['mdi-0'] = 0
175  wait_for_task_mode(linuxcnc.MODE_MDI)
176  wait_for_halui_mode('is-mdi')
177  
178  
179  sys.exit(0)
180