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 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