class PhidgetInterface:
def __init__(self):
rospy.loginfo("Initializing PhidgetInterface")
self.batteryVoltage = 0
self.forwardRange = 1
self.aftRange = 2
self.interfaceKit = InterfaceKit()
self.forwardRangeMessage = Range()
self.forwardRangeMessage.header.frame_id = 'infrared_forward'
self.forwardRangeMessage.radiation_type = Range.INFRARED
self.forwardRangeMessage.field_of_view = 0.018
self.forwardRangeMessage.min_range = 0.2
self.forwardRangeMessage.max_range = 0.8
self.forwardRangeMessage.range = 0.0
self.aftRangeMessage = Range()
self.aftRangeMessage.header.frame_id = 'infrared_aft'
self.aftRangeMessage.radiation_type = Range.INFRARED
self.aftRangeMessage.field_of_view = 0.018
self.aftRangeMessage.min_range = 0.2
self.aftRangeMessage.max_range = 0.8
self.aftRangeMessage.range = 0.0
self.sensorPublisher = rospy.Publisher('bogies', Range)
self.interfaceKit.setOnAttachHandler(self.interfaceKitAttached)
self.interfaceKit.setOnDetachHandler(self.interfaceKitDetached)
self.interfaceKit.setOnErrorhandler(self.interfaceKitError)
self.interfaceKit.setOnInputChangeHandler(self.interfaceKitInputChanged)
self.interfaceKit.setOnOutputChangeHandler(self.interfaceKitOutputChanged)
self.interfaceKit.setOnSensorChangeHandler(self.interfaceKitSensorChanged)
try:
self.interfaceKit.openPhidget()
except PhidgetException, e:
rospy.logerror("openPhidget() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
try:
self.interfaceKit.waitForAttach(10000)
except PhidgetException, e:
rospy.logerror("waitForAttach() failed")
rospy.logerror("code: %d" % e.code)
rospy.logerror("message", e.message)
raise
class PhidgetsEventThread(QtCore.QThread):
def __init__(self, parent=None):
super(PhidgetsEventThread, self).__init__(parent)
try:
self.interfaceKit = InterfaceKit()
self.interfaceKit.openPhidget()
self.interfaceKit.setOnAttachHandler(self.interfaceKitAttachedEvent)
self.interfaceKit.setOnDetachHandler(self.interfaceKitDetachEvent)
self.interfaceKit.setOnErrorhandler(self.interfaceKitErrorSlot)
self.interfaceKit.setOnInputChangeHandler(self.interfaceKitInputChangedEvent)
self.interfaceKit.setOnOutputChangeHandler(self.interfaceKitOutputChangedEvent)
self.interfaceKit.setOnSensorChangeHandler(self.interfaceKitSensorChangedEvent)
except RuntimeError as e:
print "Runtime Exception: %s" % (e.details)
print "Exiting..."
sys.exit(1)
except PhidgetException as e:
print "Phidget Exception %i: %s" % (e.code, e.details)
print "Exiting..."
sys.exit(1)
#===========================================================================
# On Interfacekit events, emit signals to main thread for updating GUI
#===========================================================================
def interfaceKitAttachedEvent(self, event):
self.emit(QtCore.SIGNAL("interfaceKitAttachedSlot"), event)
def interfaceKitDetachEvent(self, event):
self.emit(QtCore.SIGNAL("interfaceKitDetachedSlot"), event)
def interfaceKitErrorSlot(self, event):
self.emit(QtCore.SIGNAL("interfaceKitErrorSlot"), event)
def interfaceKitInputChangedEvent(self, event):
self.emit(QtCore.SIGNAL("interfaceKitInputChangedSlot"), event)
def interfaceKitOutputChangedEvent(self, event):
self.emit(QtCore.SIGNAL("interfaceKitOutputChangedSlot"), event)
def interfaceKitSensorChangedEvent(self, event):
self.emit(QtCore.SIGNAL("interfaceKitSensorChangedSlot"), event)
def setSensorSensitivity(self, value):
try:
for i in range(self.interfaceKit.getSensorCount()):
self.interfaceKit.setSensorChangeTrigger(i, value)
except PhidgetException as e:
print "Could not set sensitivity: %s" % e.details
def run(self):
self.exec_()
def setup_interfaceKit():
#Create an interfacekit object
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
interfaceKit.setOnAttachHandler(inferfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
#interfaceKit.openPhidget()
interfaceKit.openRemoteIP(IP, port=5001)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
interfaceKit.waitForAttach(10000)
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)
return interfaceKit
def setup_interfaceKit():
#Create an interfacekit object
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
interfaceKit.setOnAttachHandler(inferfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
#interfaceKit.openPhidget()
interfaceKit.openRemoteIP(IP, port=5001)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
interfaceKit.waitForAttach(10000)
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)
return interfaceKit
def run():
kit = InterfaceKit()
try:
try:
kit.openPhidget()
kit.setOnAttachHandler(attached)
kit.setOnDetachHandler(detached)
if not kit.isAttached():
print "Please attach the interface kit!"
raw_input()
except PhidgetException as e:
print ("Phidget exception %i: %s" % (e.code,e.detail))
except RuntimeError as e:
print ("Runtime error: %s" % e.message)
except Exception as e:
print ("Unknown error: %s" % (e.message))
stop(kit)
class PHIDGET_IFK(object):
""" Phidget InterfaceKit """
def __init__(self, serialNumber=None, waitForAttach=1000, **kargs):
self.interfaceKit = InterfaceKit()
if 'remoteHost' in kargs:
self.interfaceKit.openRemote(kargs['remoteHost'], serialNumber)
else:
self.interfaceKit.openPhidget(serialNumber)
self.ratiometric = 1
if 'ratiometric' in kargs:
self.ratiometric = kargs['ratiometric']
h = [
'onAttachHandler',
'onDetachHandler',
'onErrorhandler',
'onInputChangeHandler',
'onOutputChangeHandler',
'onSensorChangeHandler'
]
for event in h:
self.__dict__[event] = None
if event in kargs:
self.__dict__[event] = kargs[event]
self.interfaceKit.setOnAttachHandler(self.attached)
self.interfaceKit.setOnDetachHandler(self.detached)
self.interfaceKit.setOnErrorhandler(self.error)
self.interfaceKit.setOnInputChangeHandler(self.inputChanged)
self.interfaceKit.setOnOutputChangeHandler(self.outputChanged)
self.interfaceKit.setOnSensorChangeHandler(self.sensorChanged)
if waitForAttach > 0:
try:
self.interfaceKit.waitForAttach(waitForAttach)
except PhidgetException as e:
#print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.interfaceKit.closePhidget()
except PhidgetException as e2:
pass
raise e
def attached(self, e):
self.interfaceKit.setRatiometric(self.ratiometric)
time.sleep(0.05)
if self.onAttachHandler: self.onAttachHandler(e)
def detached(self, e):
if self.onDetachHandler: self.onDetachHandler(e)
def error(self, e):
error = {'code': e.eCode, 'description': e.description}
if self.onErrorhandler: self.onErrorhandler(error, e)
def outputChanged(self, e):
if self.onInputChangeHandler: self.onInputChangeHandler(e.index, e.state, e)
def inputChanged(self, e):
if self.onInputChangeHandler: self.onInputChangeHandler(e.index, e.state, e)
def sensorChanged(self, e):
if self.onInputChangeHandler: self.onInputChangeHandler(e.index, e.value, e)
class PowerControl(object):
''' PowerControl class wraps language around the 1014_2 -
PhidgetInterfaceKit 0/0/4 4 relay device. '''
def __init__(self):
#log.info("Start of power control object")
pass
def open_phidget(self):
''' Based on the InterfaceKit-simple.py example from Phidgets, create an
relay object, attach the handlers, open it and wait for the attachment.
This function's primarily purpose is to replace the prints with log
statements. '''
try:
self.interface = InterfaceKit()
except RuntimeError as e:
log.critical("Phidget runtime exception: %s" % e.details)
return 0
try:
self.interface.setOnAttachHandler( self.interfaceAttached )
self.interface.setOnDetachHandler( self.interfaceDetached )
self.interface.setOnErrorhandler( self.interfaceError )
except PhidgetException as e:
log.critical("Phidget Exception %i: %s" % (e.code, e.details))
return 0
try:
#print "Force open relay serial: 290968"
self.interface.openPhidget()
except PhidgetException as e:
log.critical("Phidget Exception %i: %s" % (e.code, e.details))
return 0
#log.info("Waiting for attach....")
try:
self.interface.waitForAttach(100)
except PhidgetException as e:
log.critical("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.interface.closePhidget()
except PhidgetException as e:
log.critical("Close Exc. %i: %s" % (e.code, e.details))
return 0
return 1
#Event Handler Callback Functions
def interfaceAttached(self, e):
attached = e.device
#log.info("interface %i Attached!" % (attached.getSerialNum()))
def interfaceDetached(self, e):
detached = e.device
log.info("interface %i Detached!" % (detached.getSerialNum()))
def interfaceError(self, e):
try:
source = e.device
log.critical("Interface %i: Phidget Error %i: %s" % \
(source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
log.critical("Phidget Exception %i: %s" % (e.code, e.details))
def close_phidget(self):
try:
self.interface.closePhidget()
except PhidgetException as e:
log.critical("Phidget Exception %i: %s" % (e.code, e.details))
return 0
return 1
def change_relay(self, relay=0, status=0):
''' Toggle the status of the phidget relay line to low(0) or high(1)'''
try:
self.interface.setOutputState(relay, status)
#self.emit_line_change(relay, status)
except Exception as e:
log.critical("Problem setting relay on %s" % e)
return 0
return 1
''' Convenience functions '''
def zero_on(self):
#log.info("Zero relay on")
return self.change_relay(relay=ZERO_RELAY, status=1)
def zero_off(self):
return self.change_relay(relay=ZERO_RELAY, status=0)
def one_on(self):
#log.info("one relay on")
return self.change_relay(relay=ONE_RELAY, status=1)
def one_off(self):
return self.change_relay(relay=ONE_RELAY, status=0)
def two_on(self):
#log.info("two relay on")
return self.change_relay(relay=TWO_RELAY, status=1)
def two_off(self):
return self.change_relay(relay=TWO_RELAY, status=0)
def three_on(self):
#log.info("two relay on")
return self.change_relay(relay=THREE_RELAY, status=1)
def three_off(self):
return self.change_relay(relay=THREE_RELAY, status=0)
def toggle_line(self, line=0):
''' Read the internal state of the specified line, then set the opposite
state for a toggle function'''
if not self.open_phidget():
log.critical("Problem opening phidget")
return 0
try:
curr_state = self.interface.getOutputState(line)
except Exception as e:
log.critical("Problem getting relay on %s" % e)
self.close_phidget()
return 0
if not self.change_relay(line, not curr_state):
log.critical("Problem changing relay")
return 0
if not self.close_phidget():
log.criticla("Problem closing phidget")
return 0
return 1
class PhidgetSensorHandler(AbstractSensorHandler):
def __init__(self):
self.device = None
self._attach_timeout = None
self._data_rate = None
self._sensors = None
def _try_init(self):
if all([self._data_rate, self._attach_timeout, self._sensors]):
try:
from Phidgets.Devices.InterfaceKit import InterfaceKit
from Phidgets.PhidgetException import PhidgetException
self.interface_kit = InterfaceKit()
self.interface_kit.setOnAttachHandler(lambda e: self._attach(e))
self.interface_kit.setOnDetachHandler(lambda e: self._detach(e))
self.interface_kit.setOnErrorhandler(lambda e: self._error(e))
self.interface_kit.setOnSensorChangeHandler(lambda e: self._sensor_change(e))
self.interface_kit.openPhidget()
self.interface_kit.waitForAttach(self._attach_timeout)
for i in range(self.interface_kit.getSensorCount()):
self.interface_kit.setDataRate(i, self._data_rate)
logging.info("Phidget Sensor Handler Initalized")
for s in self._sensors:
if s.port_num is not None:
s.current_data = self.interface_kit.getSensorValue(s.port_num)
logging.debug("Setting Initial Value for Sensor {} to {}".format(s.port_num, s.current_data))
else:
logging.warn("Cannot set Initial Value for Sensor {}".format(s.port_num))
except ImportError:
self.interface_kit = None
logging.error('Phidget Python Module not found. Did you install python-phidget?')
except PhidgetException as e:
self.interface_kit = None
logging.error("Could not Initalize Phidget Kit: {}".format(e.details))
def _read_sensors(self):
ready_sensors = []
for s in self._sensors:
if s.data is not None:
ready_sensors.append(s)
return ready_sensors
def _set_sensors(self, v):
logging.debug('Adding Phidget Sensors :: {}'.format(v))
self._sensors = v
self._try_init()
sensors = property(_read_sensors, _set_sensors)
attach_timeout = property(lambda self: self._attach_timeout,
lambda self, v: self._set_config('attach_timeout', v))
data_rate = property(lambda self: self._data_rate,
lambda self, v: self._set_config('data_rate', v))
def _set_config(self, prop, value):
if prop == 'data_rate':
self._data_rate = value
elif prop == 'attach_timeout':
self._attach_timeout = value
self._try_init()
def _attach(self, e):
self.device = e.device
logging.info("Phidget InterfaceKit {} Attached".format(self.device.getSerialNum()))
def _detach(self, e):
logging.warn("Phidget InterfaceKit {} Removed".format(e.device.getSerialNum()))
self.device = None
def _error(self, e):
logging.error("Phidget Error {} :: {}".format(e.eCode, e.description))
def _sensor_change(self, e):
# logging.debug("Phidget Analog Sensor Change :: Port: {} / Data: {}".format(e.index, e.value))
for s in self._sensors:
if s.port_type == 'analog' and s.port_num == e.index:
# Set a default ID if none given in config file
if s.id is None:
# Default ID is kit serial number::port
s.id = '{}:{}:{}'.format(self.device.getSerialNum(),
s.port_type, s.port_num)
s.current_data = e.value
class InterfaceKitHelper:
__inputs = np.zeros(8)
__sensors = np.zeros(8)
attached = False
led = None
def __init__(self):
# Create an interfacekit object
try:
self.__interfaceKit = InterfaceKit()
except RuntimeError as e:
print('Runtime Exception: %s' % e.details)
return 1
try:
# logging example, uncomment to generate a log file
#self.__interfaceKit.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, 'phidgetlog.log')
self.__interfaceKit.setOnAttachHandler(self.__interfaceKitAttached)
self.__interfaceKit.setOnDetachHandler(self.__interfaceKitDetached)
self.__interfaceKit.setOnErrorhandler(self.__interfaceKitError)
self.__interfaceKit.setOnInputChangeHandler(
self.__interfaceKitInputChanged)
self.__interfaceKit.setOnOutputChangeHandler(
self.__interfaceKitOutputChanged)
self.__interfaceKit.setOnSensorChangeHandler(
self.__interfaceKitSensorChanged)
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
return 1
print('[INFO] [InterfaceKitHelper] Opening phidget object....')
try:
self.__interfaceKit.openPhidget()
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
return 1
print('[INFO] [InterfaceKitHelper] Waiting for attach....')
try:
self.__interfaceKit.waitForAttach(10000)
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
try:
self.__interfaceKit.closePhidget()
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
return 1
return 1
else:
# self.__displayDeviceInfo()
pass
print(
'[INFO] [InterfaceKitHelper] Setting the data rate for each sensor index to 4ms....'
)
for i in range(self.__interfaceKit.getSensorCount()):
try:
self.__interfaceKit.setDataRate(i, 4)
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
self.led = LED(self)
def getInputs(self):
return InterfaceKitHelper.__inputs[:]
def getSensors(self):
return InterfaceKitHelper.__sensors[:]
def setOutputState(self, *args):
if self.attached:
self.__interfaceKit.setOutputState(*args)
def destroy(self):
self.attached = False
self.led.destroy()
try:
print(
'[INFO] [InterfaceKitHelper] Closing interface kit phidget...')
self.__interfaceKit.closePhidget()
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
return 1
def __displayDeviceInfo(self):
"""
Information Display Function
"""
print(
'|------------|----------------------------------|--------------|------------|'
)
print(
'|- Attached -|- Type -|- Serial No. -|- Version -|'
)
print(
'|------------|----------------------------------|--------------|------------|'
)
print('|- %8s -|- %30s -|- %10d -|- %8d -|' %
(self.__interfaceKit.isAttached(),
self.__interfaceKit.getDeviceName(),
self.__interfaceKit.getSerialNum(),
self.__interfaceKit.getDeviceVersion()))
print(
'|------------|----------------------------------|--------------|------------|'
)
print('Number of Digital Inputs: %i' %
(self.__interfaceKit.getInputCount()))
print('Number of Digital Outputs: %i' %
(self.__interfaceKit.getOutputCount()))
print('Number of Sensor Inputs: %i' %
(self.__interfaceKit.getSensorCount()))
def __interfaceKitAttached(self, e):
"""
Event Handler Callback Functions
"""
attached = e.device
print('[INFO] [InterfaceKitHelper] InterfaceKit %i Attached!' %
(attached.getSerialNum()))
self.attached = True
def __interfaceKitDetached(self, e):
detached = e.device
print('[INFO] [InterfaceKitHelper] InterfaceKit %i Detached!' %
(detached.getSerialNum()))
self.attached = False
def __interfaceKitError(self, e):
try:
source = e.device
print(
'[INFO] [InterfaceKitHelper] InterfaceKit %i: Phidget Error %i: %s'
% (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
def __interfaceKitInputChanged(self, e):
# source = e.device
# print('InterfaceKit %i: Input %i: %s' % (source.getSerialNum(), e.index, e.state))
InterfaceKitHelper.__inputs[e.index] = e.state
def __interfaceKitSensorChanged(self, e):
# source = e.device
# print('InterfaceKit %i: Sensor %i: %i' % (source.getSerialNum(), e.index, e.value))
InterfaceKitHelper.__sensors[e.index] = e.value
def __interfaceKitOutputChanged(self, e):
# source = e.device
# print('InterfaceKit %i: Output %i: %s' % (source.getSerialNum(), e.index, e.state))
pass
class IOTools:
def __init__(self, onRobot):
self.onRobot = onRobot
# Camera
self._openCam = False
# Motor Control
self._snMot = -1
self._openMot = False
self._attachedMot = False
self._cur = [0, 0]
# Servo
self._snSer = -1
self._openSer = False
self._attachedSer = False
self._limits = [0, 0]
# IF Kit
self._snIF = -1
self._openIF = False
self._attachedIF = False
self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
# LEDs
self._fistTime = True
self._status = [0, 0, 0]
self._mod = [8, 1, 1]
self._rep = [-1, 5, 6]
self._val = [True, False, False]
self._ofs = [0, 0, 0]
self._updaterThread = threading.Thread(target=self.__updateLED)
self._stop = False
self._updaterThread.setDaemon(True)
self._updaterThread.start()
def destroy(self):
# LEDs
self._stop = True
if self._attachedIF:
self._interfaceKit.setOutputState(0, 0)
self._interfaceKit.setOutputState(1, 0)
self._interfaceKit.setOutputState(2, 0)
# Servo
self.servoDisengage()
# Camera
self._openCam = False
if self._openCam:
self._cap.release()
def open(self):
self.__openIF()
self.__openMot()
self.__openSer()
self.__openCam()
###################### Camera ######################
def __openCam(self):
if not os.path.exists('/dev/video0'):
return False
self._cap = cv2.VideoCapture()
if not self._cap.open(-1):
return False
self._openCam = True
def cameraGrab(self):
if self._openCam:
return self._cap.grab()
else:
return False
def cameraRead(self):
if self._openCam:
(ret, img) = self._cap.retrieve()
return img
else:
return False
def cameraSetResolution(self, sz):
if self._openCam:
sz = sz.lower()
if sz == 'low':
self._cap.set(3, 160)
self._cap.set(4, 120)
if sz == 'medium':
self._cap.set(3, 640)
self._cap.set(4, 480)
if sz == 'high':
self._cap.set(3, 800)
self._cap.set(4, 600)
if sz == 'full':
self._cap.set(3, 1280)
self._cap.set(4, 720)
def imshow(self, wnd, img):
if not self.onRobot:
if img.__class__ != numpy.ndarray:
print "imshow - invalid image"
return False
else:
cv2.imshow(wnd, img)
cv2.waitKey(5)
####################### Servo ######################
def __openSer(self):
try:
self._advancedServo = AdvancedServo()
except RuntimeError as e:
print("Servo - Runtime Exception: %s" % e.details)
return False
try:
self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
self._advancedServo.setOnErrorhandler(self.__onErrorSer)
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
return False
try:
self._advancedServo.openPhidget()
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
return False
self._openSer = True
return True
def __onAttachedSer(self, e):
self._snSer = e.device.getSerialNum()
self._advancedServo.setServoType(
0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
self._advancedServo.setAcceleration(
0, self._advancedServo.getAccelerationMax(0))
self._advancedServo.setVelocityLimit(
0, self._advancedServo.getVelocityMax(0))
self._limits[0] = self._advancedServo.getPositionMin(0)
self._limits[1] = self._advancedServo.getPositionMax(0)
print("Servo %i Attached! Range: %f - %f" %
(self._snSer, self._limits[0], self._limits[1]))
self._attachedSer = True
def __onDetachedSer(self, e):
print("Servo %i Detached!" % (self._snSer))
self._snSer = -1
self._attachedSer = False
def __onErrorSer(self, e):
try:
source = e.device
print("Servo %i: Phidget Error %i: %s" %
(source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
def __closeSer(self):
if self._openSer == True:
self.servoDisengage()
self._advancedServo.closePhidget()
def servoEngage(self):
if self._attachedSer == True:
self._advancedServo.setEngaged(0, True)
def servoDisengage(self):
if self._attachedSer == True:
self._advancedServo.setEngaged(0, False)
def servoSet(self, pos):
if self._attachedSer == True:
self._advancedServo.setPosition(
0, min(max(pos, self._limits[0]), self._limits[1]))
############### Motor Control ######################
def __openMot(self):
try:
self._motorControl = MotorControl()
except RuntimeError as e:
print("Motor Control - Runtime Exception: %s" % e.details)
return False
try:
self._motorControl.setOnAttachHandler(self.__onAttachedMot)
self._motorControl.setOnDetachHandler(self.__onDetachedMot)
self._motorControl.setOnErrorhandler(self.__onErrorMot)
self._motorControl.setOnCurrentChangeHandler(
self.__onCurrentChangedMot)
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" %
(e.code, e.details))
return False
try:
self._motorControl.openPhidget()
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" %
(e.code, e.details))
return False
self._openMot = True
return True
def __onAttachedMot(self, e):
self._snMot = e.device.getSerialNum()
print("Motor Control %i Attached!" % (self._snMot))
self._attachedMot = True
def __onDetachedMot(self, e):
print("Motor Control %i Detached!" % (self._snMot))
self._snMot = -1
self._attachedMot = False
def __onErrorMot(self, e):
try:
source = e.device
print("Motor Control %i: Phidget Error %i: %s" %
(source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" %
(e.code, e.details))
def __onCurrentChangedMot(self, e):
self._cur[e.index] = e.current
def __closeMot(self):
if self._openMot == True:
self._motorControl.closePhidget()
def setMotors(self,
speed1=0.0,
speed2=0.0,
acceleration1=100.0,
acceleration2=100.0):
if self._openMot == True:
self._motorControl.setAcceleration(0, acceleration1)
self._motorControl.setVelocity(0, speed1)
self._motorControl.setAcceleration(1, acceleration2)
self._motorControl.setVelocity(1, speed2)
############### Interface Kit ######################
def __closeIF(self):
if self._openIF == True:
self._interfaceKit.closePhidget()
def __openIF(self):
try:
self._interfaceKit = InterfaceKit()
except RuntimeError as e:
print("IF Kit - Runtime Exception: %s" % e.details)
return False
try:
self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
self._interfaceKit.setOnSensorChangeHandler(
self.__onSensorChangedIF)
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
return False
try:
self._interfaceKit.openPhidget()
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
return False
self._openIF = True
return True
def __onAttachedIF(self, e):
self._snIF = e.device.getSerialNum()
print("InterfaceKit %i Attached!" % (self._snIF))
self._attachedIF = True
if self._fistTime:
for i in range(0, 3):
self._interfaceKit.setOutputState(0, 1)
self._interfaceKit.setOutputState(1, 1)
self._interfaceKit.setOutputState(2, 1)
time.sleep(0.1)
self._interfaceKit.setOutputState(0, 0)
self._interfaceKit.setOutputState(1, 0)
self._interfaceKit.setOutputState(2, 0)
time.sleep(0.1)
self._fistTime = False
def __onDetachedIF(self, e):
print("InterfaceKit %i Detached!" % (self._snIF))
self._snIF = -1
self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
self._attachedIF = False
def __onErrorIF(self, e):
try:
source = e.device
print("InterfaceKit %i: Phidget Error %i: %s" %
(source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
def __onSensorChangedIF(self, e):
self._sen[e.index] = e.value
def __onInputChangedIF(self, e):
self._inp[e.index] = e.state
def getSensors(self):
return self._sen
def getInputs(self):
return self._inp
################ LEDs #######################
def __updateLED(self):
t = 0
while self._stop == False:
t = (t + 1) % 100
for i in range(0, 3):
self._status[i] = ((t + self._ofs[i]) % self._mod[i]
== 0) and self._val[i] and bool(
self._rep[i])
self._rep[i] = self._rep[i] - int(self._rep[i] > 0
and self._status[i])
if self._attachedIF:
self._interfaceKit.setOutputState(i, self._status[i])
time.sleep(0.15)
def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
if mode == 'on':
self._rep[i] = -1
self._val[i] = True
self._ofs[i] = 0
self._mod[i] = 1
if mode == 'off':
self._rep[i] = -1
self._val[i] = False
self._ofs[i] = 0
self._mod[i] = 1
if mode == 'flash':
hz = min(max(hz, 1), 100)
self._rep[i] = min(max(cnt, 1), 20)
self._val[i] = True
self._ofs[i] = min(max(ofs, 0), hz)
self._mod[i] = hz
def setStatus(self, mode, hz=2, cnt=1, ofs=0):
self.__setModeLED(1, mode, hz, cnt, ofs)
def setError(self, mode, hz=2, cnt=1, ofs=0):
self.__setModeLED(2, mode, hz, cnt, ofs)
def setSemaphor(self):
self.__setModeLED(1, 'flash', 2, 6, 0)
self.__setModeLED(2, 'flash', 2, 6, 1)
def setupSensorHandlers():
global interfaceKit, batteryPublisher, rangePublisher
global batteryVoltageChannel, batteryFullLevel, batteryDepletedLevel, batteryRange, frontRangeChannel, backRangeChannel
global batteryInfo, rangeInfo
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
rospy.logfatal('Failed to create InterfaceKit: %s' % (e.details))
rospy.signal_shutdown('Failed to connect to InterfaceKit')
try:
interfaceKit.setOnAttachHandler(attachHandler)
interfaceKit.setOnDetachHandler(detachHandler)
interfaceKit.setOnErrorhandler(errorHandler)
interfaceKit.setOnInputChangeHandler(inputChangeHandler)
interfaceKit.setOnOutputChangeHandler(outputChangeHandler)
interfaceKit.setOnSensorChangeHandler(sensorChangeHandler)
except PhidgetException as e:
rospy.logfatal('Failed to set handlers: %i, %s' % (e.code, e.details))
rospy.signal_shutdown('Failed to connect to InterfaceKit')
try:
interfaceKit.openPhidget()
except PhidgetException as e:
rospy.logfatal("Failed to openPhidget() %i: %s" % (e.code, e.details))
rospy.signal_shutdown('Failed to openPhidget()')
try:
interfaceKit.waitForAttach(10000)
except PhidgetException as e:
rospy.logfatal("Failed on waitForAttach() %i: %s" %
(e.code, e.details))
interfaceKit.closePhidget()
rospy.signal_shutdown('Failed on waitForAttach()')
for sensor in range(interfaceKit.getSensorCount()):
try:
interfaceKit.setDataRate(sensor, 16)
except PhidgetException as e:
rospy.logwarn("Failed to setDateRate() %i: %s" %
(e.code, e.details))
batteryPublisher = rospy.Publisher('batteryInfo', Battery)
rangePublisher = rospy.Publisher('rangeInfo', Range)
batteryVoltageChannel = rospy.get_param('RoverState/battery/channel')
batteryFullLevel = float(rospy.get_param('RoverState/battery/fullCharge'))
batteryDepletedLevel = rospy.get_param('RoverState/battery/depletedCharge')
batteryRange = batteryFullLevel - batteryDepletedLevel
frontRangeChannel = rospy.get_param('RoverState/range/front/channel')
backRangeChannel = rospy.get_param('RoverState/range/back/channel')
sendRange()
sendBattery()
return
advancedServo.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
print("Press Enter to quit....")
chr = sys.stdin.read(1)
print("Closing...")
try:
interfaceKit.setOnAttachHandler(interfaceKitAttached)
interfaceKit_Relay.setOnAttachHandler(interfaceKitAttached) #added
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit_Relay.setOnDetachHandler(interfaceKitDetached) #added
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit_Relay.setOnErrorhandler(interfaceKitError) #added
interfaceKit.setOnInputChangeHandler(
interfaceKitInputChanged) #update info when an input changes
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
class LTCPhidget(object):
# TODO: can the remote specific events find a disconnected usb cable?
devserial = 0
IP = "0.0.0.0"
port = 0
input = {}
output = {}
sensor = {}
callback = {
'attach': [],
'detach': [],
'error': [],
'output': [],
'input': [],
'sensor': []
}
def __init__(self, **kwargs):
log.debug("Acquiring InterfaceKit")
self.ik = InterfaceKit()
log.debug("Registering Handlers")
self.ik.setOnAttachHandler(self._onAttach)
self.ik.setOnDetachHandler(self._onDetach)
self.ik.setOnErrorhandler(self._onError)
self.ik.setOnOutputChangeHandler(self._onOutput)
self.ik.setOnInputChangeHandler(self._onInput)
self.ik.setOnSensorChangeHandler(self._onSensor)
def start(self):
log.verbose("Opening remote IP")
self.ik.openRemoteIP(self.IP, self.port, self.devserial)
log.debug("Remote IP opened")
def close(self):
log.verbose("Closing InterfaceKit")
self.ik.closePhidget()
log.debug("Interfac kit closed")
def add_callback(self, cb, type):
log.debug("Adding a {} type callback".format(type))
self.callback[type].append(cb)
def remove_callback(self, cb, type):
log.debug("Removing a {} type callback".format(type))
self.callback[type].remove(cb)
def _genericCB(self, event, type):
log.verbose("{} event received".format(type))
for cb in self.callback[type]:
cb(event)
for dev in self.input.itervalues():
for cb in dev.callback[type]:
cb(event)
for dev in self.output.itervalues():
for cb in dev.callback[type]:
cb(event)
for dev in self.sensor.itervalues():
for cb in dev.callback[type]:
cb(event)
def _onAttach(self, event):
self._genericCB(event, 'attach')
def _onDetach(self, event):
self._genericCB(event, 'detach')
def _onError(self, event):
log.debug(event.description)
log.verbose("{} event received".format(type))
for cb in self.callback['error']:
cb(event)
def _onOutput(self, event):
log.verbose("Output event received")
for cb in self.callback['output']:
cb(event)
try:
for cb in self.output[event.index].callback['value']:
cb(event)
except KeyError:
pass
def _onInput(self, event):
log.verbose("Input event received")
for cb in self.callback['input']:
cb(event)
try:
for cb in self.input[event.index].callback['value']:
cb(event)
except KeyError:
pass
def _onSensor(self, event):
log.verbose("Sensor event received")
for cb in self.callback['sensor']:
cb(event)
try:
for cb in self.sensor[event.index].callback['value']:
cb(event)
except KeyError:
pass
class force_glove:
def __init__(self, log_file):
self.interfaceKit = InterfaceKit()
self.log_file = log_file
try:
self.interfaceKit.setOnAttachHandler(self.interfaceKitAttached)
self.interfaceKit.setOnDetachHandler(self.interfaceKitDetached)
self.interfaceKit.setOnErrorhandler(self.interfaceKitError)
self.interfaceKit.setOnSensorChangeHandler(self.interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Turning on glove....")
try:
self.interfaceKit.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for glove to attach....")
try:
self.interfaceKit.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
self.interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Exiting....")
exit(1)
else:
print "Glove connected..."
# self.displayDeviceInfo()
return
def setSensorRates(self):
for i in range(self.interfaceKit.getSensorCount()):
try:
self.interfaceKit.setDataRate(i, 2)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
return
def closeForceGlove(self):
try:
self.interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
print("Closed force glove...")
return
# Information Display Function
#def displayDeviceInfo(self):
# print("Glove " + self.interfaceKit.getDeviceName() + " found")
# print("Number of Sensor Inputs: %i" % (self.interfaceKit.getSensorCount()))
# Event Handler Callback Functions
def interfaceKitAttached(self, e):
attached = e.device
print("InterfaceKit %i Attached!" % (attached.getSerialNum()))
def interfaceKitDetached(self, e):
detached = e.device
print("InterfaceKit %i Detached!" % (detached.getSerialNum()))
def interfaceKitError(self, e):
try:
source = e.device
print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def interfaceKitSensorChanged(self, e):
force_data = "%i,%i" % (e.index, e.value)
self.write_log(force_data)
print("Sensor %i: %i" % (e.index, e.value))
def write_log(self, glove_data):
if glove_data is not None:
to_log = ('%f,%s\r\n') % (time.time(), glove_data)
if not self.log_file.closed:
self.log_file.write(to_log)
class PhidgetSensorHandler(AbstractSensorHandler):
def __init__(self):
self.device = None
self._attach_timeout = None
self._data_rate = None
self._sensors = None
def _try_init(self):
if all([self._data_rate, self._attach_timeout, self._sensors]):
try:
from Phidgets.Devices.InterfaceKit import InterfaceKit
from Phidgets.PhidgetException import PhidgetException
self.interface_kit = InterfaceKit()
self.interface_kit.setOnAttachHandler(
lambda e: self._attach(e))
self.interface_kit.setOnDetachHandler(
lambda e: self._detach(e))
self.interface_kit.setOnErrorhandler(lambda e: self._error(e))
self.interface_kit.setOnSensorChangeHandler(
lambda e: self._sensor_change(e))
self.interface_kit.openPhidget()
self.interface_kit.waitForAttach(self._attach_timeout)
for i in range(self.interface_kit.getSensorCount()):
self.interface_kit.setDataRate(i, self._data_rate)
logging.info("Phidget Sensor Handler Initalized")
for s in self._sensors:
if s.port_num is not None:
s.current_data = self.interface_kit.getSensorValue(
s.port_num)
logging.debug(
"Setting Initial Value for Sensor {} to {}".format(
s.port_num, s.current_data))
else:
logging.warn(
"Cannot set Initial Value for Sensor {}".format(
s.port_num))
except ImportError:
self.interface_kit = None
logging.error(
'Phidget Python Module not found. Did you install python-phidget?'
)
except PhidgetException as e:
self.interface_kit = None
logging.error("Could not Initalize Phidget Kit: {}".format(
e.details))
def _read_sensors(self):
ready_sensors = []
for s in self._sensors:
if s.data is not None:
ready_sensors.append(s)
return ready_sensors
def _set_sensors(self, v):
logging.debug('Adding Phidget Sensors :: {}'.format(v))
self._sensors = v
self._try_init()
sensors = property(_read_sensors, _set_sensors)
attach_timeout = property(
lambda self: self._attach_timeout,
lambda self, v: self._set_config('attach_timeout', v))
data_rate = property(lambda self: self._data_rate,
lambda self, v: self._set_config('data_rate', v))
def _set_config(self, prop, value):
if prop == 'data_rate':
self._data_rate = value
elif prop == 'attach_timeout':
self._attach_timeout = value
self._try_init()
def _attach(self, e):
self.device = e.device
logging.info("Phidget InterfaceKit {} Attached".format(
self.device.getSerialNum()))
def _detach(self, e):
logging.warn("Phidget InterfaceKit {} Removed".format(
e.device.getSerialNum()))
self.device = None
def _error(self, e):
logging.error("Phidget Error {} :: {}".format(e.eCode, e.description))
def _sensor_change(self, e):
# logging.debug("Phidget Analog Sensor Change :: Port: {} / Data: {}".format(e.index, e.value))
for s in self._sensors:
if s.port_type == 'analog' and s.port_num == e.index:
# Set a default ID if none given in config file
if s.id is None:
# Default ID is kit serial number::port
s.id = '{}:{}:{}'.format(self.device.getSerialNum(),
s.port_type, s.port_num)
s.current_data = e.value
def connect_to_phidget(SensorChangedFunction, serial_number=None):
#Create an interfacekit object
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
#Information Display Function
def displayDeviceInfo():
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (interfaceKit.isAttached(), interfaceKit.getDeviceName(), interfaceKit.getSerialNum(), interfaceKit.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Digital Inputs: %i" % (interfaceKit.getInputCount()))
print("Number of Digital Outputs: %i" % (interfaceKit.getOutputCount()))
print("Number of Sensor Inputs: %i" % (interfaceKit.getSensorCount()))
#Event Handler Callback Functions
def interfaceKitAttached(e):
attached = e.device
print("InterfaceKit %i Attached!" % (attached.getSerialNum()))
def interfaceKitDetached(e):
detached = e.device
print("InterfaceKit %i Detached!" % (detached.getSerialNum()))
def interfaceKitError(e):
try:
source = e.device
print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
#Main Program Code
try:
interfaceKit.setOnAttachHandler(interfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
if serial_number is not None:
interfaceKit.openPhidget(serial_number)
else:
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(10000)
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)
else:
displayDeviceInfo()
phidget = interfaceKit
return phidget
def interface_kit_sensor_changed(e):
source = e.device
print("InterfaceKit %i: Sensor %i: %i" % (source.getSerialNum(), e.index, e.value))
def interface_kit_output_changed(e):
source = e.device
print("InterfaceKit %i: Output %i: %s" % (source.getSerialNum(), e.index, e.state))
# Main Program Code
try:
# logging example, uncomment to generate a log file
# interfaceKit.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
interfaceKit.setOnAttachHandler(interface_kit_attached)
interfaceKit.setOnDetachHandler(interface_kit_detached)
interfaceKit.setOnErrorhandler(interface_kit_error)
interfaceKit.setOnInputChangeHandler(interface_kit_input_changed)
interfaceKit.setOnOutputChangeHandler(interface_kit_sensor_changed)
interfaceKit.setOnSensorChangeHandler(interface_kit_output_changed)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
interfaceKit.openRemoteIP('169.254.4.87', 5001, -1, "greenspy")
except PhidgetException as e:
def setup_phidgets():
global interfaceKits
interfaceKits = InterfaceKits()
"Print Creating phidget manager"
try:
manager = Manager()
except RuntimeError as e:
output("Runtime Exception: %s" % e.details)
output("Exiting....")
exit(1)
try:
manager.setOnAttachHandler(ManagerDeviceAttached)
manager.setOnDetachHandler(ManagerDeviceDetached)
manager.setOnErrorHandler(ManagerError)
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
output("Exiting....")
exit(1)
output("Opening phidget manager....")
logging.info("Opening phidget manager....")
try:
manager.openManager()
#manager.openRemote("hydropi","hydropi")
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
logging.error("Phidget Exception %i: %s" % (e.code, e.details))
output("Exiting....")
logging.error("Exiting....")
exit(1)
# Wait a moment for devices to attache......
output("\nWaiting one sec for devices to attach....\n\n")
logging.info("Waiting one sec for devices to attach....")
time.sleep(1)
output("Phidget manager opened.")
attachedDevices = manager.getAttachedDevices()
for attachedDevice in attachedDevices:
output("Found %30s - SN %10d" % (attachedDevice.getDeviceName(), attachedDevice.getSerialNum()))
if attachedDevice.getDeviceClass() == PhidgetClass.INTERFACEKIT:
output(" %s/%d is an InterfaceKit" % ( attachedDevice.getDeviceName(),attachedDevice.getSerialNum()))
#Create an interfacekit object
try:
newInterfaceKit = InterfaceKit()
except RuntimeError as e:
output("Runtime Exception: %s" % e.details)
output("Exiting....")
exit(1)
output(" Opening...")
try:
newInterfaceKit.openPhidget()
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
output("Exiting....")
output(" Setting handlers...")
try:
newInterfaceKit.setOnAttachHandler(interfaceKitAttached)
newInterfaceKit.setOnDetachHandler(interfaceKitDetached)
newInterfaceKit.setOnErrorhandler(interfaceKitError)
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
output("Exiting....")
exit(1)
output(" Attaching...")
try:
newInterfaceKit.waitForAttach(5000)
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
try:
newInterfaceKit.closePhidget()
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
output("Exiting....")
exit(1)
output("Exiting....")
exit(1)
output(" Setting the data rate for each sensor index to 1000ms....")
for i in range(newInterfaceKit.getSensorCount()):
try:
newInterfaceKit.setDataRate(i, 1000)
except PhidgetException as e:
output("Phidget Exception %i: %s" % (e.code, e.details))
interfaceKits.kitList.append(newInterfaceKit)
display_device_info(manager)
return manager
def main():
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
interfaceKit.setOnAttachHandler(inferfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
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(10000)
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)
print("Setting the data rate for each sensor index to 4ms....")
for i in range(interfaceKit.getSensorCount()):
try:
interfaceKit.setDataRate(i, 4)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
sys.exit(app.exec_())
try:
interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
if e.value != 1:
monitoring.insertPos(e.index)
#print("InterfaceKit %i: Sensor %i: %i" % (source.getSerialNum(), e.index, e.value))
def interfaceKitOutputChanged(e):
source = e.device
#print("InterfaceKit %i: Output %i: %s" % (source.getSerialNum(), e.index, e.state))
#Main Program Code
try:
#logging example, uncomment to generate a log file
#interfaceKit.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
interfaceKit.setOnAttachHandler(interfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
# print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
#print("Opening phidget object....")
try:
interfaceKit.openPhidget()
class SitwPhidgetsKey(wx.Frame):
def __init__(self, image, parent = None, id = -1,
pos = wx.DefaultPosition, title = sitwPara.Title):
wx.Frame.__init__(self, parent, id, title)
self.SetBackgroundColour((0, 0, 0))
self.SetSize((360, 80))
self.Center()
self.Iconize()
self.panel = wx.Panel(self, size=(320, 336))
self.panel.Bind(wx.EVT_PAINT, self.OnPaint)
#self.panel.Bind(wx.EVT_ERASE_BACKGROUND, self.OnEraseBackground)
self.Fit()
self.SampleCount = 12 #for detecting environment 12 x 10sec = 2min
self.KeyPressed = ''
self.CurAction = ''
self.CurPos = win32api.GetCursorPos()
self.PreAction = ''
self.PrePos = win32api.GetCursorPos()
self.bKeyIntervalOK = True
self.KeyMatReady = False
self.ListValMat = []
self.ListValEnv = []
self.ListValBrt = []
self.KeySearch = False
self.dtSearchStart = datetime.datetime.now()
self.dtAction = datetime.datetime.now()
self.dtRefresh = datetime.datetime.now()
self.ChannelCount = 0
self.bNight = False
self.ctEvn = 0
#self.edgeTop = 50
#self.edgeBottom = sitwPara.MoveEdgeBottom1
#self.edgeLeft = 50
#self.edgeRight = 50
self.edgeTop = 18
self.edgeBottom = 18
self.edgeLeft = 18
self.edgeRight = 18
self.strLogAction = ''
self.strLogBrightness = ''
self.subp = None
self.List_ProgramFile = []
self.List_Program = []
self.OnScreenApp = ''
#self.utSchedule = SitwScheduleTools(self)
self.utLogAction = SitwLog(self, 'logAction')
self.utLogBrightness = SitwLog(self, 'logBrightness')
#self.Bind(wx.EVT_SIZE, self.OnResize)
self.Bind(wx.EVT_CLOSE, self.OnCloseWindow)
self.Bind(wx.EVT_IDLE, self.OnIdle)
self.prtMsg('PhidgetsKey Starting...')
self.readIniFile()
self.initPhidgets()
self.initKeys()
'''collect ambient light info'''
self.timer1 = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.CheckEnv, self.timer1)
self.timer1.Start(sitwPara.SampleInterval_Env) #1000 = 1 second
'''check if there is any key has been covered'''
self.timer2 = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.CheckKey, self.timer2)
self.timer2.Start(sitwPara.SampleInterval_Key) #1000 = 1 second
'''find current on screen experience according to schedule file'''
''' -- removed --'''
'''collect sensor readings for analysis'''
if sitwPara.Log_Brightness == 'Yes':
print '<Log_Brightness On>'
self.timer5 = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.logBrightness, self.timer5)
self.timer5.Start(60 * 1000) #1000 = 1 second
self.utLogBrightness.logMsg('------ log is starting ------\t' + sitwPara.Title)
self.logBrightness(None)
else:
print '<Log_Brightness Off>'
'''collect keypad actions for analysis'''
if sitwPara.Log_Action == 'Yes':
print '<Log_Action On>'
self.timer6 = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.logActions, self.timer6)
self.timer6.Start(9 * 1000) #1000 = 1 second
self.utLogAction.logMsg('------ log is starting ------\t' + sitwPara.Title)
else:
print '<Log_Action Off>'
###Change Cursor
###http://converticon.com/
### http://stackoverflow.com/questions/7921307/temporarily-change-cursor-using-python
self.SetSystemCursor = windll.user32.SetSystemCursor #reference to function
self.SetSystemCursor.restype = c_int #return
self.SetSystemCursor.argtype = [c_int, c_int] #arguments
self.LoadCursorFromFile = windll.user32.LoadCursorFromFileA #reference to function
self.LoadCursorFromFile.restype = c_int #return
self.LoadCursorFromFile.argtype = c_char_p #arguments
CursorPath ='../pic/handr.cur'
NewCursor = self.LoadCursorFromFile(CursorPath)
if NewCursor is None:
print "Error loading the cursor"
elif self.SetSystemCursor(NewCursor, win32con.IDC_ARROW) == 0:
print "Error in setting the cursor"
###Change Cursor
self.DimScreen = wx.DisplaySize()
print 'Screen Dimensions: ' + str(self.DimScreen[0]) + ' x ' + str(self.DimScreen[1])
##########################################
'''m,b parameters for each sensor'''
for i in range(len(sitwPara.List_mb)):
print '===== ', sitwPara.List_mb[i][0], sitwPara.List_mb[i][1]
##########################################
### not in use at the moment
def OnEraseBackground(self, event):
return True
def OnPaint(self, event):
'''
# establish the painting surface
dc = wx.PaintDC(self.panel)
dc.SetPen(wx.Pen('blue', 4))
# draw a blue line (thickness = 4)
dc.DrawLine(50, 20, 300, 20)
dc.SetPen(wx.Pen('red', 1))
# draw a red rounded-rectangle
rect = wx.Rect(50, 50, 100, 100)
dc.DrawRoundedRectangleRect(rect, 8)
# draw a red circle with yellow fill
dc.SetBrush(wx.Brush('yellow'))
x = 250
y = 100
r = 50
dc.DrawCircle(x, y, r)
'''
#dc = wx.PaintDC(self.panel)
dc = wx.BufferedPaintDC(self.panel)
dc.SetBackground(wx.BLUE_BRUSH)
dc.Clear()
self.onDraw(dc)
def onDraw(self, dc):
strColorPen1 = 'red'
strColorPen2 = 'blue'
for i in range(sitwPara.KeyCount):
rect = sitwPara.List_ButtonPos[i]
dc.SetBrush(wx.Brush((0, 255*self.ListValBrt[i], 255*self.ListValBrt[i])))
if self.KeyPressed == sitwPara.List_Action[i]:
dc.SetPen(wx.Pen(strColorPen1, 5))
else:
dc.SetPen(wx.Pen(strColorPen2, 1))
#dc.SetPen(wx.TRANSPARENT_PEN)
dc.DrawRoundedRectangleRect(rect, 8)
def initPhidgets(self):
try:
self.interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
self.interfaceKit.setOnAttachHandler(self.inferfaceKitAttached)
self.interfaceKit.setOnDetachHandler(self.interfaceKitDetached)
self.interfaceKit.setOnErrorhandler(self.interfaceKitError)
self.interfaceKit.setOnInputChangeHandler(self.interfaceKitInputChanged)
self.interfaceKit.setOnOutputChangeHandler(self.interfaceKitOutputChanged)
self.interfaceKit.setOnSensorChangeHandler(self.interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
self.interfaceKit.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
try:
self.interfaceKit.waitForAttach(10000)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.closePhidgets()
else:
self.displayDeviceInfo()
#get sensor count
try:
self.ChannelCount = self.interfaceKit.getSensorCount()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
self.closePhidgets()
sitwPara.KeyCount = 0 #no sensor has been detected
self.prtMsg(' ****** No sensor has been detected !!!\n')
print("Setting the data rate for each sensor index to 4ms....")
for i in range(sitwPara.KeyCount):
try:
self.interfaceKit.setDataRate(i, 4)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
### depends on the low light performance of the sensor
print("Setting the sensitivity for each sensor index to ???....")
for i in range(sitwPara.KeyCount):
try:
self.interfaceKit.setSensorChangeTrigger(i, 2) #~~~~*YL*~~~~
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def closePhidgets(self):
#print("Press Enter to quit....")
#chr = sys.stdin.read(1)
#print("Closing...")
try:
self.interfaceKit.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
#print("Done.")
#exit(1)
def initKeys(self):
self.KeyPressed = ''
self.ListValMat = []
for i in range(sitwPara.KeyCount):
self.ListValEnv = []
for j in range(self.SampleCount):
self.ListValEnv.append(self.readSensorValue(i))
self.ListValMat.append(self.ListValEnv)
self.KeyMatReady = True
self.ListValBrt = []
for i in range(sitwPara.KeyCount):
self.ListValBrt.append(0)
def readIniFile(self):
self.prtMsg('Read system ini file...')
try:
config = ConfigParser.ConfigParser()
config.readfp(open(sitwPara.FilePath_Ini))
for eachIniData in self.iniData():
Section = eachIniData[0]
Keys = eachIniData[1]
for Key in Keys:
val = config.get(Section, Key)
if (Section == "General"):
if (Key == "KeyCount"):
sitwPara.KeyCount = int(val)
elif (Key == "Sensitivity"):
sitwPara.Sensitivity = float(val)
elif (Key == "MovingPace"):
sitwPara.MovingPace = int(val)
elif (Key == "SampleInterval_Key"):
sitwPara.SampleInterval_Key = int(val)
elif (Key == "SampleInterval_Env"):
sitwPara.SampleInterval_Env = int(val)
elif (Key == "Log_Action"):
sitwPara.Log_Action = str(val)
elif (Key == "Log_Brightness"):
sitwPara.Log_Brightness = str(val)
else:
pass
print('[' + Section + '] ' + Key + ' = ' + val)
continue
except: #IOError
self.prtMsg('Error: readIniFile()')
finally:
pass
def iniData(self):
return (("General",
("KeyCount",
"Sensitivity",
"MovingPace",
"SampleInterval_Key",
"SampleInterval_Env",
"Log_Action",
"Log_Brightness")),)
#Information Display Function
def displayDeviceInfo(self):
print("|------------|----------------------------------|--------------|------------|")
print("|- Attached -|- Type -|- Serial No. -|- Version -|")
print("|------------|----------------------------------|--------------|------------|")
print("|- %8s -|- %30s -|- %10d -|- %8d -|" % (self.interfaceKit.isAttached(), self.interfaceKit.getDeviceName(), self.interfaceKit.getSerialNum(), self.interfaceKit.getDeviceVersion()))
print("|------------|----------------------------------|--------------|------------|")
print("Number of Digital Inputs: %i" % (self.interfaceKit.getInputCount()))
print("Number of Digital Outputs: %i" % (self.interfaceKit.getOutputCount()))
print("Number of Sensor Inputs: %i" % (self.interfaceKit.getSensorCount()))
#Event Handler Callback Functions
def inferfaceKitAttached(self, e):
attached = e.device
print("InterfaceKit %i Attached!" % (attached.getSerialNum()))
def interfaceKitDetached(self, e):
detached = e.device
print("InterfaceKit %i Detached!" % (detached.getSerialNum()))
def interfaceKitError(self, e):
try:
source = e.device
print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
def interfaceKitInputChanged(self, e):
source = e.device
print("InterfaceKit %i: Input %i: %s" % (source.getSerialNum(), e.index, e.state))
def interfaceKitSensorChanged(self, e):
if not self.KeyMatReady:
return
#source = e.device
#print("InterfaceKit %i: Sensor %i: %i" % (source.getSerialNum(), e.index, e.value))
if not self.KeySearch:
self.KeySearch = True
self.dtSearchStart = datetime.datetime.now()
def interfaceKitOutputChanged(self, e):
source = e.device
print("InterfaceKit %i: Output %i: %s" % (source.getSerialNum(), e.index, e.state))
def readSensorValue(self, channel_id):
val = 0.0
try:
val = self.interfaceKit.getSensorValue(channel_id)
#val = self.interfaceKit.getSensorRawValue(channel_id) / 4.095
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
#val = 1.478777 * float(val) + 33.67076 #------ for 1142_0: Lux = m * SensorValue + b
#val = math.exp(0.02385 * float(val) - 0.56905) #------ for 1143_0: Lux = math.exp(m * SensorValue + b)
#val = math.exp(sitwPara.List_mb[channel_id][0] * float(val) + sitwPara.List_mb[channel_id][1]) #------ for 1143_0: Lux = math.exp(m * SensorValue + b)
return val
def CheckEnv(self, event):
for i in range(sitwPara.KeyCount):
ValIns = self.readSensorValue(i)
ValEnv = sum(self.ListValMat[i]) / len(self.ListValMat[i])
if (float(ValIns) / float(ValEnv)) > 0.70 or self.ctEvn >= 2: #natural change or the big change keeps for long time
self.ListValMat[i].pop(0)
self.ListValMat[i].append(ValIns)
self.ctEvn = 0
else:
self.ctEvn += 1 #ignore those suddenly changes
###test
#for val in self.ListValMax:
# print val, '->', sum(val)/len(val)
#print '-------------------------------------------------------'
def CheckKey(self, event):
if not self.KeySearch:
return
dtCurrentTime = datetime.datetime.now()
if dtCurrentTime - self.dtSearchStart > datetime.timedelta(microseconds = 6000000): # =6s ; 1000000 = 1s
self.KeySearch = False
if dtCurrentTime - self.dtAction > datetime.timedelta(microseconds = 500000): # =0.5s ; 1000000 = 1s
self.bKeyIntervalOK = True
else:
self.bKeyIntervalOK = False
self.bNight = True
for i in range(sitwPara.KeyCount):
ValIns = self.readSensorValue(i)
ValEnv = sum(self.ListValMat[i]) / len(self.ListValMat[i])
if ValEnv > 20.0:
self.bNight = False
self.ListValBrt[i] = float(ValIns) / float(ValEnv)
if self.ListValBrt[i] > 1.0:
self.ListValBrt[i] = 1.0
#sort
#self.ListValBrt.sort(cmp = None, key = None, reverse = False)
if self.bNight == False:
NightFactor = 1.0
else:
NightFactor = 1.8
#KeyID = self.ListValBrt.index(min(self.ListValBrt))
for i in range(sitwPara.KeyCount):
KeyID = sitwPara.List_KeyCheckOrder[i]
if self.ListValBrt[KeyID] < (sitwPara.Sensitivity * NightFactor):
bKey = True
break
else:
bKey = False
if bKey == True:
if KeyID == 1:
if self.KeyPressed == 'left' or self.bKeyIntervalOK == True:
self.KeyPressed = 'left'
self.dtAction = datetime.datetime.now()
else:
self.KeyPressed = ''
elif KeyID == 2:
if self.KeyPressed == 'right' or self.bKeyIntervalOK == True:
self.KeyPressed = 'right'
self.dtAction = datetime.datetime.now()
else:
self.KeyPressed = ''
elif KeyID == 3:
if self.KeyPressed == 'up' or self.bKeyIntervalOK == True:
self.KeyPressed = 'up'
self.dtAction = datetime.datetime.now()
else:
self.KeyPressed = ''
elif KeyID == 4:
if self.KeyPressed == 'down' or self.bKeyIntervalOK == True:
self.KeyPressed = 'down'
self.dtAction = datetime.datetime.now()
else:
self.KeyPressed = ''
elif KeyID == 0:
if self.KeyPressed != 'click' and self.KeyPressed != 'clicked' and self.KeyPressed != 'dclick' and self.KeyPressed != 'dclicked' and self.bKeyIntervalOK == True:
self.KeyPressed = 'click'
self.dtAction = datetime.datetime.now()
elif self.KeyPressed != 'dclick' and self.KeyPressed != 'dclicked' and (dtCurrentTime - self.dtAction > datetime.timedelta(microseconds = 2200000)): # =2.2s ; 1000000 = 1s
self.KeyPressed = 'dclick'
self.dtAction = datetime.datetime.now()
else:
self.KeyPressed = ''
if len(self.KeyPressed) > 0:
###filter
#-- removed ---
###filter
pos = win32api.GetCursorPos()
if self.KeyPressed == 'up':
if pos[1] >= self.edgeTop:
#nx = (pos[0]) * 65535.0 / self.DimScreen[0]
#ny = (pos[1] - sitwPara.MovingPace) * 65535.0 / self.DimScreen[1]
win32api.mouse_event(win32con.MOUSEEVENTF_MOVE, 0, -sitwPara.MovingPace)
#win32api.SetCursorPos((pos[0], pos[1] - sitwPara.MovingPace))
elif self.KeyPressed == 'down':
if pos[1] <= self.DimScreen[1] - self.edgeBottom:
#nx = (pos[0]) * 65535.0 / self.DimScreen[0]
#ny = (pos[1] + sitwPara.MovingPace) * 65535.0 / self.DimScreen[1]
win32api.mouse_event(win32con.MOUSEEVENTF_MOVE, 0, +sitwPara.MovingPace)
#win32api.SetCursorPos((pos[0], pos[1] + sitwPara.MovingPace))
elif self.KeyPressed == 'left':
if pos[0] >= self.edgeLeft:
#nx = (pos[0] - sitwPara.MovingPace) * 65535.0 / self.DimScreen[0]
#ny = (pos[1]) * 65535.0 / self.DimScreen[1]
win32api.mouse_event(win32con.MOUSEEVENTF_MOVE, -sitwPara.MovingPace, 0)
#win32api.SetCursorPos((pos[0] - sitwPara.MovingPace, pos[1]))
elif self.KeyPressed == 'right':
if pos[0] <= self.DimScreen[0] - self.edgeRight:
#nx = (pos[0] + sitwPara.MovingPace) * 65535.0 / self.DimScreen[0]
#ny = (pos[1]) * 65535.0 / self.DimScreen[1]
win32api.mouse_event(win32con.MOUSEEVENTF_MOVE, +sitwPara.MovingPace, 0)
#win32api.SetCursorPos((pos[0] + sitwPara.MovingPace, pos[1]))
elif self.KeyPressed == 'click':
self.click(pos[0], pos[1])
#print '**********click**************'
self.KeyPressed = 'clicked'
elif self.KeyPressed == 'dclick':
self.dclick(pos[0], pos[1])
#print '**********dclick**************'
self.KeyPressed = 'dclicked'
###Action Log
if sitwPara.Log_Action == 'Yes':
self.CurAction = self.KeyPressed
self.CurPos = win32api.GetCursorPos()
if self.CurAction != self.PreAction and len(self.PreAction) > 0:
#dtCurrentTime = datetime.datetime.now()
#strTimeTag = datetime.datetime.strftime(dtCurrentTime, '%Y-%m-%d %H:%M:%S')
#pos = win32api.GetCursorPos()
strLog = self.PreAction + '\t(' + str(self.PrePos[0]) + ',' + str(self.PrePos[1]) + ')' \
+ '\t' + self.OnScreenApp
self.utLogAction.logMsg(strLog)
self.PreAction = self.CurAction
self.PrePos = self.CurPos
def click(self, x,y):
win32api.SetCursorPos((x,y))
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTDOWN,x,y,0,0)
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTUP,x,y,0,0)
def dclick(self, x,y):
win32api.SetCursorPos((x,y))
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTDOWN,x,y,0,0)
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTUP,x,y,0,0)
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTDOWN,x,y,0,0)
win32api.mouse_event(win32con.MOUSEEVENTF_LEFTUP,x,y,0,0)
def logBrightness(self, event):
strValEnv = ''
for i in range(sitwPara.KeyCount):
ValEnv = sum(self.ListValMat[i]) / len(self.ListValMat[i])
strValEnv += str('%06.2f'%(ValEnv)) + '\t'
self.utLogBrightness.logMsg(strValEnv)
self.utLogBrightness.wrtLog(False)
def logActions(self, event):
self.utLogAction.wrtLog(False)
def GetSchedule(self, event):
# --removed --
pass
def FindAppName(self, event):
# --removed --
pass
def prtMsg(self, strMsg):
dtCurrentTime = datetime.datetime.now()
strTimeTag = datetime.datetime.strftime(dtCurrentTime, '%Y-%m-%d %H:%M:%S')
#strTimeTag += str('%03d'%(int(datetime.datetime.strftime(dtCurrentTime, '%f'))/1000))
print(strTimeTag + ' ' + strMsg + "\n")
def OnIdle(self, event):
if not self.KeySearch:
return
dtCurrentTime = datetime.datetime.now()
if dtCurrentTime - self.dtRefresh > datetime.timedelta(microseconds = 200000): #1000000 = 1s
self.dtRefresh = dtCurrentTime
self.panel.Refresh(eraseBackground = False)
pass
def OnCloseWindow(self, event):
print "Do something b4 closing..."
###Change Cursor
CursorPath = "../pic/arrow.cur"
NewCursor = self.LoadCursorFromFile(CursorPath)
if NewCursor is None:
print "Error loading the cursor"
elif self.SetSystemCursor(NewCursor, win32con.IDC_ARROW) == 0:
print "Error in setting the cursor"
###Change Cursor
self.closePhidgets()
self.prtMsg('Destroy Phidgets Key')
if sitwPara.Log_Action == 'Yes':
self.utLogAction.wrtLog(True) #force to log all messages
if sitwPara.Log_Brightness == 'Yes':
self.utLogBrightness.wrtLog(True) #force to log all messages
#time.sleep(1)
self.Destroy()
class IOTools:
def __init__(self, onRobot):
self.onRobot = onRobot
# Camera
self._openCam=False
# Motor Control
self._snMot=-1
self._openMot=False
self._attachedMot=False
self._cur = [0, 0]
# Servo
self._snSer=-1
self._openSer=False
self._attachedSer=False
self._limits = [0, 0]
# IF Kit
self._snIF=-1
self._openIF=False
self._attachedIF=False
self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
# LEDs
self._fistTime = True
self._status = [0,0,0]
self._mod = [8, 1, 1]
self._rep = [-1, 5, 6]
self._val = [True, False, False]
self._ofs = [0, 0, 0]
self._updaterThread = threading.Thread(target=self.__updateLED)
self._stop = False
self._updaterThread.setDaemon(True)
self._updaterThread.start()
def destroy(self):
# LEDs
self._stop = True
if self._attachedIF:
self._interfaceKit.setOutputState(0, 0)
self._interfaceKit.setOutputState(1, 0)
self._interfaceKit.setOutputState(2, 0)
# Servo
self.servoDisengage()
# Camera
self._openCam = False
if self._openCam:
self._cap.release()
def open(self):
self.__openIF()
self.__openMot()
self.__openSer()
self.__openCam()
###################### Camera ######################
def __openCam(self):
if not os.path.exists('/dev/video0'):
return False
self._cap = cv2.VideoCapture()
if not self._cap.open(-1):
return False
self._openCam = True
def cameraGrab(self):
if self._openCam:
return self._cap.grab()
else:
return False
def cameraRead(self):
if self._openCam:
(ret, img)=self._cap.retrieve()
return img
else:
return False
def cameraSetResolution(self, sz):
"""
:rtype : object
"""
if self._openCam:
sz=sz.lower()
if sz=='low':
self._cap.set(3,160)
self._cap.set(4,120)
if sz=='medium':
self._cap.set(3,640)
self._cap.set(4,480)
if sz=='high':
self._cap.set(3,800)
self._cap.set(4,600)
if sz=='full':
self._cap.set(3,1280)
self._cap.set(4,720)
def imshow(self, wnd, img):
if not self.onRobot:
if img.__class__ != numpy.ndarray:
print "imshow - invalid image"
return False
else:
cv2.imshow(wnd,img)
cv2.waitKey(5)
####################### Servo ######################
def __openSer(self):
try:
self._advancedServo = AdvancedServo()
except RuntimeError as e:
print("Servo - Runtime Exception: %s" % e.details)
return False
try:
self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
self._advancedServo.setOnErrorhandler(self.__onErrorSer)
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
return False
try:
self._advancedServo.openPhidget()
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
return False
self._openSer=True
return True
def __onAttachedSer(self,e):
self._snSer = e.device.getSerialNum()
self._advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
self._advancedServo.setAcceleration(0, self._advancedServo.getAccelerationMax(0))
self._advancedServo.setVelocityLimit(0, self._advancedServo.getVelocityMax(0))
self._limits[0] = self._advancedServo.getPositionMin(0)
self._limits[1] = self._advancedServo.getPositionMax(0)
print("Servo %i Attached! Range: %f - %f" % (self._snSer, self._limits[0], self._limits[1]))
self._attachedSer=True
def __onDetachedSer(self,e ):
print("Servo %i Detached!" % (self._snSer))
self._snSer = -1
self._attachedSer=False
def __onErrorSer(self, e):
try:
source = e.device
print("Servo %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
def __closeSer(self):
if self._openSer==True:
self.servoDisengage()
self._advancedServo.closePhidget()
def servoEngage(self):
if self._attachedSer==True:
self._advancedServo.setEngaged(0, True)
def servoDisengage(self):
if self._attachedSer==True:
self._advancedServo.setEngaged(0, False)
def servoSet(self, pos):
if self._attachedSer==True:
self._advancedServo.setPosition(0, min(max(pos,self._limits[0]),self._limits[1]))
############### Motor Control ######################
def __openMot(self):
try:
self._motorControl = MotorControl()
except RuntimeError as e:
print("Motor Control - Runtime Exception: %s" % e.details)
return False
try:
self._motorControl.setOnAttachHandler(self.__onAttachedMot)
self._motorControl.setOnDetachHandler(self.__onDetachedMot)
self._motorControl.setOnErrorhandler(self.__onErrorMot)
self._motorControl.setOnCurrentChangeHandler(self.__onCurrentChangedMot)
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
return False
try:
self._motorControl.openPhidget()
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
return False
self._openMot=True
return True
def __onAttachedMot(self,e):
self._snMot = e.device.getSerialNum()
print("Motor Control %i Attached!" % (self._snMot))
self._attachedMot=True
def __onDetachedMot(self,e ):
print("Motor Control %i Detached!" % (self._snMot))
self._snMot = -1
self._attachedMot=False
def __onErrorMot(self, e):
try:
source = e.device
print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
def __onCurrentChangedMot(self, e):
self._cur[e.index] = e.current
def __closeMot(self):
if self._openMot==True:
self._motorControl.closePhidget()
def setMotors(self, speed1=0.0, speed2=0.0, acceleration1=100.0, acceleration2=100.0 ):
if self._openMot==True:
self._motorControl.setAcceleration(0, acceleration1)
self._motorControl.setVelocity(0, speed1)
self._motorControl.setAcceleration(1, acceleration2)
self._motorControl.setVelocity(1, speed2)
############### Interface Kit ######################
def __closeIF(self):
if self._openIF==True:
self._interfaceKit.closePhidget()
def __openIF(self):
try:
self._interfaceKit = InterfaceKit()
except RuntimeError as e:
print("IF Kit - Runtime Exception: %s" % e.details)
return False
try:
self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
self._interfaceKit.setOnSensorChangeHandler(self.__onSensorChangedIF)
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
return False
try:
self._interfaceKit.openPhidget()
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
return False
self._openIF=True
return True
def __onAttachedIF(self,e):
self._snIF = e.device.getSerialNum()
print("InterfaceKit %i Attached!" % (self._snIF))
self._attachedIF=True
if self._fistTime:
for i in range(0,3):
self._interfaceKit.setOutputState(0, 1)
self._interfaceKit.setOutputState(1, 1)
self._interfaceKit.setOutputState(2, 1)
time.sleep(0.1)
self._interfaceKit.setOutputState(0, 0)
self._interfaceKit.setOutputState(1, 0)
self._interfaceKit.setOutputState(2, 0)
time.sleep(0.1)
self._fistTime = False
def __onDetachedIF(self,e ):
print("InterfaceKit %i Detached!" % (self._snIF))
self._snIF = -1
self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
self._attachedIF=False
def __onErrorIF(self, e):
try:
source = e.device
print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
def __onSensorChangedIF(self, e):
self._sen[e.index] = e.value
def __onInputChangedIF(self, e):
self._inp[e.index] = e.state
def getSensors(self):
"""
:rtype : object
"""
return self._sen;
def getInputs(self):
return self._inp;
################ LEDs #######################
def __updateLED(self):
t=0
while self._stop==False:
t=(t+1)%100
for i in range(0,3):
self._status[i]=((t+self._ofs[i])%self._mod[i]==0) and self._val[i] and bool(self._rep[i])
self._rep[i]=self._rep[i]-int(self._rep[i]>0 and self._status[i])
if self._attachedIF:
self._interfaceKit.setOutputState(i, self._status[i])
time.sleep(0.15)
def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
if mode=='on':
self._rep[i]=-1
self._val[i]=True
self._ofs[i]=0
self._mod[i]=1
if mode=='off':
self._rep[i]=-1
self._val[i]=False
self._ofs[i]=0
self._mod[i]=1
if mode=='flash':
hz=min(max(hz,1),100)
self._rep[i]=min(max(cnt,1),20)
self._val[i]=True
self._ofs[i]=min(max(ofs,0),hz)
self._mod[i]=hz
def setStatus(self, mode, hz=2, cnt=1, ofs=0):
self.__setModeLED(1,mode, hz, cnt, ofs)
def setError(self, mode, hz=2, cnt=1, ofs=0):
self.__setModeLED(2,mode, hz, cnt, ofs)
def setSemaphor(self):
self.__setModeLED(1,'flash', 2, 6, 0)
self.__setModeLED(2,'flash', 2, 6, 1)
class Node(object):
name = None
sensors = []
outputs = []
inputs = []
triggers = []
clocks = []
repeaters = []
interface_kit = None
def __init__(self, name, *args, **kwargs):
self.name = name
self.initializing = True
if LIVE: self.interface_kit = InterfaceKit()
self.manager = PubSub(self, pub_port=settings.NODE_PUB, sub_port=settings.NODE_SUB, sub_filter=self.name)
self.logger = util.get_logger("%s.%s" % (self.__module__, self.__class__.__name__))
self.initialize()
self.run()
def initialize(self):
while self.initializing:
self.logger.info("Waiting for manager")
json = dict(name=self.name, method='add_node')
self.publish(json)
gevent.sleep(1)
return
def publish(self, message):
message['name'] = self.name
message['method'] = message.get('method', 'node_change')
self.manager.publish(aes.encrypt(json.dumps(message, cls=ComplexEncoder), settings.KEY))
self.test_triggers(message)
def test_triggers(self, message):
for t in self.triggers:
t.handle_event(message)
def initialize_rpc(self, obj, **kwargs):
rpc = zmq.Context()
rpc_socket = rpc.socket(zmq.REP)
rpc_socket.bind("tcp://*:%s" % obj.get('port'))
self.logger.info("RPC listening on: %s" % obj.get('port'))
settings.KEY = base64.urlsafe_b64decode(str(obj.get('key')))
self.logger.info("%s Initialized" % self.name)
while True:
if self.initializing:
self.initializing = False
self.publish(dict(method='initialized'))
message = aes.decrypt(rpc_socket.recv(), settings.KEY)
ob = json.loads(message)
try:
res = getattr(self, ob.get("method"))(ob)
st = json.dumps(res, cls=ComplexEncoder)
rpc_socket.send(aes.encrypt(st, settings.KEY))
except Exception as e:
self.logger.exception(e)
gevent.sleep(.1)
def hello(self, obj):
return self.json()
def get_sensor(self, index):
for sensor in self.sensors:
if sensor.index == index: return sensor
return False
def get_output(self, index):
for output in self.outputs:
if output.index == index: return output
return False
def get_input(self, index):
for input in self.inputs:
if input.index == index: return input
return False
def get_sensor_values(self, ob):
res = {}
for sensor in self.sensors:
res[sensor.id] = sensor.json()
return res
def get_output_values(self):
res = {}
for output in self.outputs:
res[ouput.id] = output.json()
return res
def set_output_state(self, ob):
output = self.get_output(ob.get('index'))
if output:
self.logger.info("%s: turning %s to %s index: %s" % (self.name, ob.get('type'), ob.get('state'), output.index))
output.set_state(ob.get('state'))
return dict(state=output.current_state)
def json(self, ob=None):
return dict(
name=self.name,
sensors=[s.json() for s in self.sensors],
outputs=[o.json() for o in self.outputs],
inputs=[i.json() for i in self.inputs],
triggers=[t.json() for t in self.triggers],
repeaters=[r.json() for r in self.repeaters],
clocks=[c.json() for c in self.clocks],
cls=self.__class__.__name__
)
def __conform__(self, protocol):
return json.dumps(self.json(), cls=ComplexEncoder)
def displayDeviceInfo(self):pass
#Event Handler Callback Functions
def inferfaceKitAttached(self, e):
attached = e.device
self.logger.info("InterfaceKit %i Attached!" % (attached.getSerialNum()))
def interfaceKitDetached(self, e):
detached = e.device
self.logger.info("InterfaceKit %i Detached!" % (detached.getSerialNum()))
def interfaceKitError(self, e):
try:
if e.eCode not in (36866,):
source = e.device
self.logger.info("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
except PhidgetException as e:
self.logger.exception(e)
def interfaceKitInputChanged(self, e):
input = self.get_input(e.index)
if not input: return
val = input.do_conversion(e.value)
ob = input.json()
self.publish(ob)
self.logger.info("%s Input: %s" % (input.display, val))
def interfaceKitSensorChanged(self, e):
sensor = self.get_sensor(e.index)
if not sensor: return
val = sensor.do_conversion(float(e.value)) if sensor else 0
ob = sensor.json()
self.publish(ob)
self.logger.info("%s Sensor: %s" % (sensor.display, val))
def interfaceKitOutputChanged(self, e):
output = self.get_output(e.index)
if not output: return
output.current_state = e.state
ob = output.json()
self.publish(ob)
self.logger.info("%s Output: %s" % (output.display, output.current_state))
def run(self):
if LIVE: self.init_kit()
while True: gevent.sleep(.1)
def init_kit(self):
try:
self.interface_kit.setOnAttachHandler(self.inferfaceKitAttached)
self.interface_kit.setOnDetachHandler(self.interfaceKitDetached)
self.interface_kit.setOnErrorhandler(self.interfaceKitError)
self.interface_kit.setOnInputChangeHandler(self.interfaceKitInputChanged)
self.interface_kit.setOnOutputChangeHandler(self.interfaceKitOutputChanged)
self.interface_kit.setOnSensorChangeHandler(self.interfaceKitSensorChanged)
except PhidgetException as e:
self.logger.exception(e)
self.logger.info("Opening phidget object....")
try:
self.interface_kit.openPhidget()
except PhidgetException as e:
self.logger.exception(e)
self.logger.info("Waiting for attach....")
try:
self.interface_kit.waitForAttach(10000)
except PhidgetException as e:
self.logger.exception(e)
try:
self.interface_kit.closePhidget()
except PhidgetException as e:
self.logger.exception(e)
self.logger.info("Exiting....")
exit(1)
self.logger.info("Exiting....")
else:
self.displayDeviceInfo()
self.logger.info("Initializing Sensors")
for i in range(self.interface_kit.getSensorCount()):
try:
sensor = self.get_sensor(i)
if sensor:
self.logger.info("Setting Up: %s" % sensor.display)
self.logger.info("Change: %s" % sensor.change)
self.logger.info("Data Rate: %s" % sensor.data_rate)
self.interface_kit.setSensorChangeTrigger(i, sensor.change)
self.interface_kit.setDataRate(i, sensor.data_rate)
except PhidgetException as e:
self.logger.exception(e)
if __name__ == "__main__":
# Set the timeout alarm signal handler
signal.signal(signal.SIGALRM, handler)
#Create an interfacekit object
try:
interfaceKit = InterfaceKit()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
interfaceKit.setOnAttachHandler(inferfaceKitAttached)
interfaceKit.setOnDetachHandler(interfaceKitDetached)
interfaceKit.setOnErrorhandler(interfaceKitError)
interfaceKit.setOnInputChangeHandler(interfaceKitInputChanged)
interfaceKit.setOnOutputChangeHandler(interfaceKitOutputChanged)
interfaceKit.setOnSensorChangeHandler(interfaceKitSensorChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget relay board....")
try:
interfaceKit.openRemoteIP("192.168.128.2", port=5001, serial=SERIAL_NUM)
except PhidgetException as e:
class LTCPhidget(object):
# TODO: can the remote specific events find a disconnected usb cable?
devserial = 0
IP = "0.0.0.0"
port = 0
input = {}
output = {}
sensor = {}
callback = {'attach': [],
'detach': [],
'error': [],
'output': [],
'input': [],
'sensor': []}
def __init__(self, **kwargs):
log.debug("Acquiring InterfaceKit")
self.ik = InterfaceKit()
log.debug("Registering Handlers")
self.ik.setOnAttachHandler(self._onAttach)
self.ik.setOnDetachHandler(self._onDetach)
self.ik.setOnErrorhandler(self._onError)
self.ik.setOnOutputChangeHandler(self._onOutput)
self.ik.setOnInputChangeHandler(self._onInput)
self.ik.setOnSensorChangeHandler(self._onSensor)
def start(self):
log.verbose("Opening remote IP")
self.ik.openRemoteIP(self.IP, self.port, self.devserial)
log.debug("Remote IP opened")
def close(self):
log.verbose("Closing InterfaceKit")
self.ik.closePhidget()
log.debug("Interfac kit closed")
def add_callback(self, cb, type):
log.debug("Adding a {} type callback".format(type))
self.callback[type].append(cb)
def remove_callback(self, cb, type):
log.debug("Removing a {} type callback".format(type))
self.callback[type].remove(cb)
def _genericCB(self, event, type):
log.verbose("{} event received".format(type))
for cb in self.callback[type]:
cb(event)
for dev in self.input.itervalues():
for cb in dev.callback[type]:
cb(event)
for dev in self.output.itervalues():
for cb in dev.callback[type]:
cb(event)
for dev in self.sensor.itervalues():
for cb in dev.callback[type]:
cb(event)
def _onAttach(self, event):
self._genericCB(event, 'attach')
def _onDetach(self, event):
self._genericCB(event, 'detach')
def _onError(self, event):
log.debug(event.description)
log.verbose("{} event received".format(type))
for cb in self.callback['error']:
cb(event)
def _onOutput(self, event):
log.verbose("Output event received")
for cb in self.callback['output']:
cb(event)
try:
for cb in self.output[event.index].callback['value']:
cb(event)
except KeyError:
pass
def _onInput(self, event):
log.verbose("Input event received")
for cb in self.callback['input']:
cb(event)
try:
for cb in self.input[event.index].callback['value']:
cb(event)
except KeyError:
pass
def _onSensor(self, event):
log.verbose("Sensor event received")
for cb in self.callback['sensor']:
cb(event)
try:
for cb in self.sensor[event.index].callback['value']:
cb(event)
except KeyError:
pass