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