class CameraStation(object): def __init__(self, serial_number, yservo, zservo): self.controller = AdvancedServo() self.serial_number = serial_number self.yservo = yservo self.zservo = zservo def initServo(self, index, vel): self.controller.setServoType(index, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) self.controller.setEngaged(index, False) maxAcceleration = self.controller.getAccelerationMax(index) maxVelocity = self.controller.getVelocityMax(index) self.controller.setAcceleration(index, maxAcceleration * 0.5) self.controller.setVelocityLimit(index, maxVelocity * vel / 100) self.controller.setEngaged(index, True) self.controller.setPosition(index, 90) sleep(0.5) print "Servo {} initialized.".format(index) def init(self, yvel, zvel): # Opening servo controller self.controller.openPhidget(self.serial_number) self.controller.waitForAttach(HARDWARE_CONNECTION_TIMEOUT) self.initServo(self.yservo, yvel) self.initServo(self.zservo, zvel) def setPosition(self, yangle, zangle): # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) def setMoveToPosition(self, yangle, zangle): # Calculate movement time for both servos dy = abs(yangle - self.controller.getPosition(self.yservo)) dz = abs(zangle - self.controller.getPosition(self.zservo)) ty = dy / self.controller.getVelocityLimit(self.yservo) tz = dz / self.controller.getVelocityLimit(self.zservo) # Set new position for each servo self.controller.setPosition(self.yservo, yangle) self.controller.setPosition(self.zservo, zangle) # Wait until motors sets to desire positions sleep(max(ty, tz)) def start(self): self.controller.setEngaged(self.yservo, True) self.controller.setEngaged(self.zservo, True) def stop(self): self.controller.setEngaged(self.yservo, False) self.controller.setEngaged(self.zservo, False) def __del__(self): self.stop() self.controller.closePhidget()
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)
print("Estado Servo 0 Detenido: %s" % advancedServo.getStopped(0)) print("Estado Servo 1 Detenido: %s" % advancedServo.getStopped(1)) print("Estado Servo 0 Engranado: %s" % advancedServo.getEngaged(0)) print("Estado Servo 1 Engranado: %s" % advancedServo.getEngaged(1)) print("Trabajando con motores 0 y 1...") print("Aceleracion ajustada a un cuarto de la maxima: %f" % (advancedServo.getAccelerationMax(0) / 4)) advancedServo.setAcceleration(0, (advancedServo.getAccelerationMax(0) / 4)) advancedServo.setAcceleration(1, (advancedServo.getAccelerationMax(1) / 4)) sleep(0.5) print("Velocidad ajustada a un cuarto de la maxima: %f" % (advancedServo.getVelocityMax(0) / 4)) advancedServo.setVelocityLimit(0, (advancedServo.getVelocityMax(0) / 4)) advancedServo.setVelocityLimit(1, (advancedServo.getVelocityMax(1) / 4)) sleep(0.5) print("Motores a posicion neutral (90 grados)...") advancedServo.setPosition(0, 90.00) advancedServo.setPosition(1, 90.00) advancedServo.setEngaged(0, True) advancedServo.setEngaged(1, True) sleep(0.5) print("Estado Servo 0 Engranado: %s" % advancedServo.getEngaged(0)) print("Estado Servo 1 Engranado: %s" % advancedServo.getEngaged(1)) print("Ajuste posicion minima a 30 grados")
except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Opening phidget object....") try: advancedServo.openPhidget() except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Exiting....") exit(1) advancedServo.waitForAttach(10000) advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) advancedServo.setAcceleration(0, advancedServo.getAccelerationMax(0)) advancedServo.setVelocityLimit(0, advancedServo.getVelocityMax(0)) advancedServo.setEngaged(0, True) time.sleep(0.5) advancedServo.setPosition(0, advancedServo.getPositionMin(0)) #servo in default position. """Now the rest of the stuff""" #Get the list of instruments connected to the computer. insts= visa.get_instruments_list() print "Instruments found:", (insts) #User chooses which instruments are which. list starts with 0. scopenumber = int(raw_input('Which instrument is scope? Type number...' )) bigskynumber = int(raw_input('Which instrument is laser? Type number...' )) scopeid = str(insts[scopenumber]).strip('[]') bigskyid = str(insts[bigskynumber]).strip('[]')
class ServoControl: __ready = False def __init__(self): # Create an advancedServo object try: self.__advancedServo = AdvancedServo() except RuntimeError as e: print('[ERROR] [ServoControl] Runtime Exception: %s' % e.details) return 1 # set up our event handlers try: # logging example, uncomment to generate a log file #self.__advancedServo.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, 'phidgetlog.log') self.__advancedServo.setOnAttachHandler(self.__attached) self.__advancedServo.setOnDetachHandler(self.__detached) self.__advancedServo.setOnErrorhandler(self.__error) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Opening phidget object....') try: self.__advancedServo.openPhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 print('[INFO] [ServoControl] Waiting for attach....') try: self.__advancedServo.waitForAttach(10000) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) try: self.__advancedServo.closePhidget() except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) return 1 return 1 else: # self.__DisplayDeviceInfo() pass def engage(self): if self.__ready: try: print('[INFO] [ServoControl] Engaging servo...') self.__advancedServo.setEngaged(0, True) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def disengage(self): if self.__ready: try: print('[INFO] [ServoControl] Disengaging servo...') self.__advancedServo.setEngaged(0, False) self.__ready = False except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def setPosition(self, position): if self.__ready: try: position = np.clip(position, self.__advancedServo.getPositionMin(0), self.__advancedServo.getPositionMax(0)) print('[INFO] [ServoControl] Moving servo to {}'.format( position)) self.__advancedServo.setPosition(0, position) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def destroy(self): print('[INFO] [ServoControl] Closing advanced servo phidget...') try: self.disengage() self.__advancedServo.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.__advancedServo.isAttached(), self.__advancedServo.getDeviceName(), self.__advancedServo.getSerialNum(), self.__advancedServo.getDeviceVersion())) print( '|------------|----------------------------------|--------------|------------|' ) print('Number of motors: %i' % (self.__advancedServo.getMotorCount())) def __attached(self, e): """ Event Handler Callback Functions """ attached = e.device print('[INFO] [ServoControl] Servo %i Attached!' % (attached.getSerialNum())) try: 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.__ready = True except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details)) def __detached(self, e): detached = e.device print('[INFO] [ServoControl] Servo %i Detached!' % (detached.getSerialNum())) self.__ready = False def __error(self, e): try: source = e.device print('[ERROR] Phidget Error %i: %s' % (source.getSerialNum(), e.eCode, e.description)) except PhidgetException as e: print('[ERROR] Phidget Exception %i: %s' % (e.code, e.details))
try: print("Setting the servo type for motor 0 to HITEC_HS322HD") advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) #Setting custom servo parameters example - 600us-2000us == 120 degrees, velocity max 1500 #advancedServo.setServoParameters(0, 600, 2000, 120, 1500) print("Speed Ramping state: %s" % advancedServo.getSpeedRampingOn(0)) print("Stopped state: %s" % advancedServo.getStopped(0)) print("Engaged state: %s" % advancedServo.getEngaged(0)) print("Working with motor 0 only...") print("Adjust Acceleration to maximum: %f" % advancedServo.getAccelerationMax(0)) advancedServo.setAcceleration(0, advancedServo.getAccelerationMax(0)) sleep(2) print("Adjust Velocity Limit to maximum: %f" % advancedServo.getVelocityMax(0)) advancedServo.setVelocityLimit(0, advancedServo.getVelocityMax(0)) sleep(2) print("Engage the motor...") advancedServo.setEngaged(0, True) sleep(2) print("Engaged state: %s" % advancedServo.getEngaged(0)) print("Move to position positionMin...") advancedServo.setPosition(0, advancedServo.getPositionMin(0)) sleep(5) print("Move to position PositionMax...") advancedServo.setPosition(0, advancedServo.getPositionMax(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)
except PhidgetException, e: print("Phidget Exception %i: %s" % (e.code, e.details)) try: servo.closePhidget() except PhidgetException, e: PhidgetError(e) print("Exiting....") exit(1) else: DisplayDeviceInfo() try: #set servos types and accelerations, set engaged so we can use them. servo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) servo.setAcceleration(0, servo.getAccelerationMax(0) / 2) servo.setVelocityLimit(0, servo.getVelocityMax(0) / 2) servo.setEngaged(0, True) servo.setServoType(1, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) servo.setAcceleration(1, servo.getAccelerationMax(1) / 2) servo.setVelocityLimit(1, servo.getVelocityMax(1) / 2) servo.setEngaged(1, True) # authentication setup conn = httplib.HTTPConnection(HOST) base64string = base64.encodestring('%s:%s' % (KEY, KEYSECRET))[:-1] authheader = "Basic %s" % base64string headers = {'Authorization': authheader} #subscribe first and get the subscribe ID conn.request("POST", "/api/control/sub/" + SENSOR_NAME,
s1.openPhidget(serial=99590) m1.waitForAttach(10000) m2.waitForAttach(10000) m3.waitForAttach(10000) s1.waitForAttach(10000) print("Done\n") except Exception as e: print str(e) exit(1) # Setup motors and servos print("Setting up motors and servo...") s1.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) s1.setEngaged(0, True) s1.setAcceleration(0, s1.getAccelerationMax(0)) s1.setVelocityLimit(0, s1.getVelocityMax(0)) s1.setPosition(0, s1.getPositionMin(0)) setAllMotorsAcceleration(25) servoScale = s1.getPositionMax(0) - s1.getPositionMin(0) print("Done\n") # Setup joystick try: print("Setting up joystick...") Joystick = joystick.Joystick(0) Joystick.init() print("Connected to Joystick %s" % Joystick.get_id()) except Exception as e: print str(e) print("Error: No Joystick") exit(1)