class Controller(): """Implements Phidget Controller""" def __init__(self): try: self.advancedServo = AdvancedServo() self.advancedServo.openPhidget() print("Waiting to be attached") self.advancedServo.waitForAttach(10000) print("Initialize Servo") self.advancedServo.setVelocityLimit(0, 180.00) self.advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD) print("Engage Servo") self.advancedServo.setEngaged(0, True) print("Ready") except RuntimeError as e: print("Runtime Exception: %s" % e.details) print("Exiting....") exit(1) def windMax(self): self.advancedServo.setPosition(0, self.advancedServo.getPositionMax(0)) def windMin(self): self.advancedServo.setPosition(0, self.advancedServo.getPositionMin(0)) sleep(3) def disengage(self): self.advancedServo.setEngaged(0, False) sleep(3) def close(self): self.advancedServo.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("Phidget Exception %i: %s" % (e.code, e.details)) # Setup Phidget servo = AdvancedServo() servo.setOnErrorhandler(Error) # Open Phidget servo.openPhidget() servo.waitForAttach(500) # Set Phidget servo parameters try: motor = 0 servo.setEngaged(0, True) servo_min = servo.getPositionMin(motor) + 5 servo_max = servo.getPositionMax(motor) - 5 servo_mid = (servo_max - servo_min)/2 servo.setAcceleration(motor, 500) # I just picked these to be smooth, feel free to change servo.setVelocityLimit(motor, 2000) except PhidgetException as e: print("Phidget Exception %i: %s" % (e.code, e.details)) print("Exiting....") exit(1) # Set up STB sample_rate = 25 # This works best with low sampling rates sensor = hapticsstb.STB(sample_rate) sensor.bias() # Tares the sensor, make sure that nothing's touching the board print sensor.bias_vector
#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("Engage the motor...") advancedServo.setEngaged(0, True) sleep(2) print("Engaged state: %s" % advancedServo.getEngaged(0)) print("Move to position PositionMax...") advancedServo.setPosition(0, advancedServo.getPositionMax(0)) sleep(10) fh = open("/Users/u5212257/trigger", "w") fh.write("a") fh.close() sleep(0.5) print("Move to position Check 1") advancedServo.setServoParameters(0, 800, 1600, 330, 25) sleep(10) print("Move to position PositionMin...") advancedServo.setPosition(0, advancedServo.getPositionMin(0)) sleep(10)
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))
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)) sleep(5) print("Adjust Acceleration to 50...") advancedServo.setAcceleration(0, 50.00) sleep(2) print("Adjust Velocity Limit to 50...") advancedServo.setVelocityLimit(0, 50.00) sleep(2) print("Move to position PositionMin...") advancedServo.setPosition(0, advancedServo.getPositionMin(0)) sleep(5) print("Disengage the motor...")
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)
if pos>0: advancedServo.setPosition(0, advancedServo.getPosition(0)-1) # else: advancedServo.setPosition(0, advancedServo.getPosition(0)+1) # else: print "M1 getEngaged(0) = False" Start() print "Try Start M1" def Close(): advancedServo.closePhidget() Init() min_pos = advancedServo.getPositionMin(0) max_pos = advancedServo.getPositionMax(0) Start() #--------------------------------------------------------------------------------------------------------------------------------------------------------------- def target_m1(m1_conn): while(True): cmd = m1_conn.recv() if cmd[0:4]=='stop': Stop() elif cmd[0:5] == 'start': Start() elif cmd[0:4] == 'init': Init() elif cmd[0:5] == 'close': Close() elif cmd[0:1] == '1': try:
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) # Websocket handler