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)
except PhidgetException as e:
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("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)
print("Disengage the motor...")
advancedServo.setEngaged(0, False)
sleep(2)
print("Engaged state: %s" % advancedServo.getEngaged(0))
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
try:
advancedServo.closePhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
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 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))
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...")
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 advancedServo.getEngaged(0):
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':
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)
