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("Ajustando el tipo de motor para un HITEC_HS322HD")
advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
advancedServo.setServoType(1, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
print("Estado Speed Ramping del servo 0: %s" %
advancedServo.getSpeedRampingOn(0))
print("Estado Speed Ramping del servo 1: %s" %
advancedServo.getSpeedRampingOn(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)
print("Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
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('[]')
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))
exit(1)
else:
DisplayDeviceInfo()
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))
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)
servo.waitForAttach(10000)
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",
m3.openPhidget(serial=398770)
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")
