class SensorThread(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
# Create
try:
print "Setting up sensor interface"
self.device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
# Open
try:
# Hook our function above into the device object
self.device.setOnSensorChangeHandler(sensorChanged)
self.device.openPhidget()
except PhidgetException as e:
print ("Phidget Exception %i: %s" % (e.code, e.detail))
exit(1)
def run(self):
self.running = True
print "Sensors> Waiting for attach"
try:
self.device.setOnAttachHandler(AttachHandler)
except PhidgetException as e:
print "Phidget Exception %i" % (e.code)
pass
def stop(self):
self.running = False
self.device.closePhidget()
class phidget:
def __init__(self, id, port):
self.id = id
self.port = port
self.serial_numbers = (
262305, # 4-port, shelf 6
262295, # 4-port, shelf 6
318471, # 8-port, shelf 2
312226, # 8-port, shelf 4
259764, # 4-port, shelf 8
319749, # 8-port, shelf 10
259763, # 4-port, shelf 6
)
self.serial_num = self.serial_numbers[id]
self.lockfile = "/var/run/lock/phidget-%s.lock" % self.id
self.lock_fp = open(self.lockfile, "w")
self.lock_fd = self.lock_fp.fileno()
fcntl.lockf(self.lock_fd, fcntl.LOCK_EX)
if debug:
print "[%.2f] Aquired lock for Phidget %s" % (time.time(), self.id)
try:
self.device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
try:
self.device.openPhidget(serial=self.serial_num)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.detail))
fcntl.lockf(self.lock_fd, fcntl.LOCK_UN)
exit(1)
self.device.waitForAttach(10000)
#print ("Phidget %d attached!" % (self.device.getSerialNum()))
#print ("Phidget has %d inputs, %d outputs, %d sensors"
# %(self.device.getInputCount(), self.device.getOutputCount(), self.device.getSensorCount()))
def close(self):
self.device.closePhidget()
fcntl.lockf(self.lock_fd, fcntl.LOCK_UN)
if debug:
print "[%.2f] Released lock for Phidget %s" % (time.time(),
self.id)
def on(self):
self.device.setOutputState(self.port, 1)
def off(self):
self.device.setOutputState(self.port, 0)
def cmd(self, cmd):
if cmd == 'on':
self.on()
if cmd == 'off':
self.off()
class phidget:
def __init__(self, id, port):
self.id = id
self.port = port
self.serial_numbers = (262305, # 4-port, shelf 6
262295, # 4-port, shelf 6
318471, # 8-port, shelf 2
312226, # 8-port, shelf 4
259764, # 4-port, shelf 8
319749, # 8-port, shelf 10
259763, # 4-port, shelf 6
)
self.serial_num = self.serial_numbers[id]
self.lockfile = "/var/run/lock/phidget-%s.lock" %self.id
self.lock_fp = open(self.lockfile, "w")
self.lock_fd = self.lock_fp.fileno()
fcntl.lockf(self.lock_fd, fcntl.LOCK_EX)
if debug:
print "[%.2f] Aquired lock for Phidget %s" %(time.time(), self.id)
try:
self.device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
try:
self.device.openPhidget(serial=self.serial_num)
except PhidgetException as e:
print ("Phidget Exception %i: %s" % (e.code, e.detail))
fcntl.lockf(self.lock_fd, fcntl.LOCK_UN)
exit(1)
self.device.waitForAttach(10000)
#print ("Phidget %d attached!" % (self.device.getSerialNum()))
#print ("Phidget has %d inputs, %d outputs, %d sensors"
# %(self.device.getInputCount(), self.device.getOutputCount(), self.device.getSensorCount()))
def close(self):
self.device.closePhidget()
fcntl.lockf(self.lock_fd, fcntl.LOCK_UN)
if debug:
print "[%.2f] Released lock for Phidget %s" %(time.time(), self.id)
def on(self):
self.device.setOutputState(self.port, 1)
def off(self):
self.device.setOutputState(self.port, 0)
def cmd(self, cmd):
if cmd == 'on':
self.on()
if cmd == 'off':
self.off()
def main():
parser = argparse.ArgumentParser(description='read sensor values from a Phidgets device')
group = parser.add_mutually_exclusive_group(required=True)
group.add_argument('-d', '--digital', metavar='<input id>', type=int, help='read from a digital input')
group.add_argument('-a', '--analog', metavar='<sensor id>', type=int, help='read from an analog sensor')
parser.add_argument('-t', '--temperature-sensor', action='store_const', const=True, default=False, help='device is a temperature sensor, output in degrees celcius. must be reading an analog sensor')
parser.add_argument('-m', '--precision-light-sensor-multiplier', metavar='<multiplier>', type=float, help='\'m\' value from the underside of the precision light sensor. must be reading an analog sensor')
parser.add_argument('-b', '--precision-light-sensor-boost', metavar='<boost>', type=float, help='\'b\' value from the underside of the precision light sensor. must be reading an analog sensor')
cli_args = parser.parse_args()
try:
device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
exit(2)
try:
device.openPhidget()
device.waitForAttach(1000)
if cli_args.digital != None:
if cli_args.digital < device.getInputCount():
print device.getInputState(cli_args.digital)
else:
parser.print_usage()
print('%s: error: input id out of range (maximum is %d)' % (sys.argv[0], device.getInputCount()-1))
elif cli_args.analog != None:
if cli_args.analog < device.getSensorCount():
if cli_args.temperature_sensor == True:
print (float(device.getSensorValue(cli_args.analog)) * 0.2222) - 61.1111
elif (cli_args.precision_light_sensor_multiplier == None) != (cli_args.precision_light_sensor_boost == None):
parser.print_usage()
print('%s: error: you must supply both precision light sensor arguments')
elif cli_args.precision_light_sensor_multiplier != None and cli_args.precision_light_sensor_boost != None:
print (cli_args.precision_light_sensor_multiplier * float(device.getSensorValue(cli_args.analog)) + cli_args.precision_light_sensor_boost)
else:
print device.getSensorValue(cli_args.analog)
else:
parser.print_usage()
print('%s: error: sensor id out of range (maximum is %d)' % (sys.argv[0], device.getSensorCount()-1))
device.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
device.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
exit(0)
try:
interfaceKit.openPhidget()
except PhidgetException, e:
print "Phidget Exception %i: %s" % (e.code, e.message)
print "Exiting...."
exit(1)
print "Waiting for attach...."
try:
interfaceKit.waitForAttach(10000)
except PhidgetException, e:
print "Phidget Exception %i: %s" % (e.code, e.message)
try:
interfaceKit.closePhidget()
except PhidgetException, e:
print "Phidget Exception %i: %s" % (e.code, e.message)
print "Exiting...."
exit(1)
print "Exiting...."
exit(1)
print "Output Count: "+str(interfaceKit.getOutputCount())
time.sleep(3)
print "Output 0 is currently in state "+str(interfaceKit.getOutputState(0))
print "Press Enter to toggle output 0"
chr = sys.stdin.read(1)
try:
interfaceKit.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
interfaceKit.waitForAttach(86400000) # wait a day
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
chr = sys.stdin.read(1)
print("Closing...")
try:
interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
try:
device.setOnAttachHandler(AttachHandler)
device.setOnDetachHandler(DetachHandler)
device.openPhidget()
device.setOnSensorChangeHandler(sensorChanged)
except PhidgetException as e:
LocalErrorCatcher(e)
try:
pub = rospy.Publisher('sonar', Float32)
rospy.init_node('sonar')
except rospy.ROSInterruptException:
pass
print("Phidget Simple Playground (plug and unplug devices)");
print("Press Enter to end anytime...");
character = str(raw_input())
print("Closing...")
# f = open('sonar.txt','a')
# f.write('********************************\n')
# f.close()
try:
device.closePhidget()
except PhidgetException as e:
LocalErrorCatcher(e)
print('</table></td><td>')
#Print the Digital Input values
print('<b>Digital Input Values</b>')
print('<table border="1"><tr><td>Input Number</td><td>Input Value</td></tr>\n')
for i in range (0,8):
try:
print ('<tr><td>'+str(i)+'</td><td>'+str(ifk.getInputState(i))+'</td></tr>\n')
except:
pint('<tr><td>'+str(i)+'</td><td>Could not Read Digital Input State </td></tr>\n')
print('</table></td><td>')
#Print the Digital Output States
print('<b>Digital Output States</b>')
print('<table border="1"><tr><td>Output Number</td><td>Output State</td><td>Set True</td><td>Set False</td></tr>\n')
for i in range (0,8):
try:
print ('<tr><td>'+str(i)+'</td><td>'+str(ifk.getOutputState(i))+'</td><td><a href="SetTrue'+str(i)+'.py">Set True</a></td><td><a href="SetFalse'+str(i)+'.py">Set False</a></td></tr>\n')
except:
pint('<tr><td>'+str(i)+'</td><td>Could not Read Digital Outputt State </td></tr>\n')
print('</table></td></tr></table>')
print ('<br><br>This page does not automaticly refresh')
print ('<br><br><a href="WebInterfaceKit.py">Refresh this page</a>')
print('</body></html>\n')
ifk.closePhidget()
exit(0)
class Sonar:
def __init__(self):
try:
self.device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
self.height = 0
self.vel = 0
self.last_hit_time = 0
self.lock = Lock()
num = [0.0181,0.0543,0.0543,0.0181]
den = [1,-1.76,1.1829,-0.2781]
self.vzfilter = IIRfilter(num,den)
try:
self.device.setOnAttachHandler(self.AttachHandler)
self.device.setOnDetachHandler(self.DetachHandler)
self.device.openPhidget()
self.device.setOnSensorChangeHandler(self.sensorChanged)
except PhidgetException as e:
self.LocalErrorCatcher(e)
def AttachHandler(self,event):
attachedDevice = event.device
serialNumber = attachedDevice.getSerialNum()
deviceName = attachedDevice.getDeviceName()
attachedDevice.setRatiometric(True)
attachedDevice.setSensorChangeTrigger(5,1)
print("Hello to Device " + str(deviceName) + ", Serial Number: " + str(serialNumber))
def DetachHandler(self,event):
detachedDevice = event.device
serialNumber = detachedDevice.getSerialNum()
deviceName = detachedDevice.getDeviceName()
print("Goodbye Device " + str(deviceName) + ", Serial Number: " + str(serialNumber))
def sensorChanged(self,e):
# f = open('sonar.txt','a')
with self.lock:
current_hit = time.time()
time_elapse = current_hit-self.last_hit_time
if time_elapse > 0.1:
#print ("Sensor height {0} cm vel {1} cm/s".format(self.height, self.vel))
self.last_hit_time= current_hit
self.vel = (e.value*1024.0/1000-self.height)/time_elapse
self.vzfilter.add_raw_data(self.vel)
self.vel = self.vzfilter.get_filter_data()
self.height = e.value*1024.0/1000
# print ("Sensor %i: %i" % (e.index, height))
# f.write('Height at %.3f\n',height)
# f.close()
return 0
def LocalErrorCatcher(event):
print("Phidget Exception: " + str(event.code) + " - " + str(event.details) + ", Exiting...")
exit(1)
def get_height(self):
#with self.lock:
return self.height
def get_vel(self):
#with self.lock:
return self.vel
def close_phidget(self):
print("Closing...")
# f = open('sonar.txt','a')
# f.write('********************************\n')
# f.close()
try:
self.device.closePhidget()
except PhidgetException as e:
self.LocalErrorCatcher(e)
