def AttachInterfaceKit(databasepath, serialNumber):
def inputChangeHandler(event):
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO INTERFACEKIT_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.state))
conn.commit()
conn.close()
def outputChangeHandler(event):
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO INTERFACEKIT_OUTPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.state))
conn.commit()
conn.close()
def sensorChangeHandler(event):
conn = sqlite3.connect(databasepath)
conn.execute(
"INSERT INTO INTERFACEKIT_SENSORCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.value))
conn.commit()
conn.close()
try:
ik = InterfaceKit()
ik.setOnInputChangeHandler(inputChangeHandler)
ik.setOnOutputChangeHandler(outputChangeHandler)
ik.setOnSensorChangeHandler(sensorChangeHandler)
ik.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
class InterfaceSource(Source):
def __init__(self):
Source.__init__(self)
try:
self._device = InterfaceKit()
except RuntimeError as e:
print("Runtime Error: %s" % e.message)
try:
self._device.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.detail))
self._device.setOnSensorChangeHandler(self.sensor_changed)
print("Phidget: Waiting for Connection")
self._device.waitForAttach(10000)
self._device.setSensorChangeTrigger(0, 0)
self._device.setDataRate(0, 1)
print("Phidget: Connected")
def sensor_changed(self, e):
if self.sink is not None:
self.sink([e.value])
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 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()
except PhidgetException as e:
#print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
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 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 Phidgets:
def __init__(self):
self.V1 = 0.0
self.V2list = [
0, 0, 0, 0, 0, 0, 0
] #lista dei valori associati alle porte analogiche, corrispondenza porta analogica posizione nella lista
self.running = True
def ponteDiWheatstone(self):
R1 = 1000.0 #1KOhm
R2 = 1000.0 #1KOhm
R3 = 1000.0 #1KOhm
Vs = 5.0
for pos, v in enumerate(self.V2list):
if v == 0:
continue
Vm = v - self.V1 #ingresso differenziale
print Vm
#formula per il calcolo della resistenza
p1 = R3 / (R1 + R3) + (Vm / Vs)
p2 = 1 - p1
Rx = R2 * (p1 / p2)
#
date = datetime.now().strftime('%Y-%m-%d')
time = datetime.now().strftime('%H:%M:%S')
self.monitoring.insertValue(Rx, pos + 1, date, time)
#
print("la resistenza sull'input %d vale %f: " % (pos + 1, Rx))
def partitoreDiTensione(self, val):
if val != 1:
r1 = 1000
Vref = 12 #5V tensione applicata al partitore
V2 = (5.0 / 1023.0) * val #tensione letta ai capi della resistenza
r2 = r1 * V2 / (Vref - V2)
print("la resistenza vale %f: " % r2)
def interfaceKitSensorChanged(self, e):
source = e.device
if e.value == 1: #1 indica che non sta nulla collegato e resetto anche il suo valore nella lista
if e.index == 0:
self.V1 = 0
else:
self.V2list[e.index - 1] = 0
return
if e.index == 0: #V1 va collegato sempre sul pin 0
self.V1 = (5.0 / 1023.0) * e.value
print self.V1
else:
val = (5.0 / 1023.0) * e.value
self.V2list[
e.index -
1] = val #corrispondenza porta analogica posizione nella lista
print str(self.V2list)
self.ponteDiWheatstone()
#if e.value != 1:
# self.monitoring.insertPos(e.index)
#print("InterfaceKit %i: Sensor %i: %i" % (source.getSerialNum(), e.index, e.value))
def run(self):
try:
self.interfaceKit = InterfaceKit(
) #instanzio la classe per interfacciarmi con la scheda di acquisizione
self.monitoring = sm.SensorMonitoring(
) #tabella dove sono memorizzati i valori dei sensori collegati
self.monitoring.open()
self.monitoring.create()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
self.interfaceKit.setOnSensorChangeHandler(
self.interfaceKitSensorChanged)
except PhidgetException as e:
print("Exiting....")
exit(1)
try:
self.interfaceKit.openPhidget()
except PhidgetException as e:
print("Exiting....")
exit(1)
try:
self.interfaceKit.waitForAttach(10000)
except PhidgetException as e:
try:
self.interfaceKit.closePhidget()
except PhidgetException as e:
exit(1)
print(
"Assicurarsi di aver collegato la scheda di acquisizione Phidget 1019 al PC. Exiting...."
)
exit(1)
else:
print "Scheda di acquisizione rilevata."
for i in range(self.interfaceKit.getSensorCount()):
try:
self.interfaceKit.setDataRate(i, 4)
except PhidgetException as e:
pass
def close(self):
self.running = False
self.monitoring.close()
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(0)
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
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)
class Phidgets:
def __init__(self):
self.V1 = 0.0
self.V2list = [0,0,0,0,0,0,0] #lista dei valori associati alle porte analogiche, corrispondenza porta analogica posizione nella lista
self.running = True
def ponteDiWheatstone(self):
R1 = 1000.0#1KOhm
R2 = 1000.0#1KOhm
R3 = 1000.0#1KOhm
Vs = 5.0
for pos, v in enumerate(self.V2list):
if v==0:
continue
Vm = v-self.V1 #ingresso differenziale
print Vm
#formula per il calcolo della resistenza
p1 = R3/(R1+R3) + (Vm/Vs)
p2 = 1 - p1
Rx = R2*(p1/p2);
#
date = datetime.now().strftime('%Y-%m-%d')
time = datetime.now().strftime('%H:%M:%S')
self.monitoring.insertValue(Rx, pos+1, date,time)
#
print ("la resistenza sull'input %d vale %f: " % (pos+1, Rx))
def partitoreDiTensione(self, val):
if val != 1:
r1 = 1000
Vref = 12 #5V tensione applicata al partitore
V2 = (5.0/1023.0)*val #tensione letta ai capi della resistenza
r2 = r1*V2/(Vref-V2)
print ("la resistenza vale %f: " % r2)
def interfaceKitSensorChanged(self, e):
source = e.device
if e.value == 1:#1 indica che non sta nulla collegato e resetto anche il suo valore nella lista
if e.index==0:
self.V1=0
else:
self.V2list[e.index-1]=0
return
if e.index == 0:#V1 va collegato sempre sul pin 0
self.V1 = (5.0/1023.0)*e.value
print self.V1
else:
val = (5.0/1023.0)*e.value
self.V2list[e.index-1]=val #corrispondenza porta analogica posizione nella lista
print str(self.V2list)
self.ponteDiWheatstone()
#if e.value != 1:
# self.monitoring.insertPos(e.index)
#print("InterfaceKit %i: Sensor %i: %i" % (source.getSerialNum(), e.index, e.value))
def run(self):
try:
self.interfaceKit = InterfaceKit() #instanzio la classe per interfacciarmi con la scheda di acquisizione
self.monitoring = sm.SensorMonitoring() #tabella dove sono memorizzati i valori dei sensori collegati
self.monitoring.open()
self.monitoring.create()
except RuntimeError as e:
print("Runtime Exception: %s" % e.details)
print("Exiting....")
exit(1)
try:
self.interfaceKit.setOnSensorChangeHandler(self.interfaceKitSensorChanged)
except PhidgetException as e:
print("Exiting....")
exit(1)
try:
self.interfaceKit.openPhidget()
except PhidgetException as e:
print("Exiting....")
exit(1)
try:
self.interfaceKit.waitForAttach(10000)
except PhidgetException as e:
try:
self.interfaceKit.closePhidget()
except PhidgetException as e:
exit(1)
print("Assicurarsi di aver collegato la scheda di acquisizione Phidget 1019 al PC. Exiting....")
exit(1)
else:
print "Scheda di acquisizione rilevata."
for i in range(self.interfaceKit.getSensorCount()):
try:
self.interfaceKit.setDataRate(i, 4)
except PhidgetException as e:
pass
def close(self):
self.running = False
self.monitoring.close()
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(0)
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 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()
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:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
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
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 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)
#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:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Waiting for attach....")
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
