def setupMoveService():
"""Initialize the PhidgetLinear service
Establish a connection with the Phidget 1065 Motor Control and
then with the ROS Master as the service PhidgetLinear
"""
rospy.init_node('PhidgetLinear', log_level=rospy.DEBUG)
serial_no = rospy.get_param("~serial_no", 0)
if not serial_no == 0:
rospy.loginfo("Using motor controller with serial number %d",
serial_no)
else:
rospy.loginfo(
"No serial number specified. This is fine for systems with one controller, but may behave unpredictably for systems with multiple motor controllers"
)
global motorControl, minAcceleration, maxAcceleration, timer, invert_speed, posdataPub
timer = 0
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnDetachHandler(mcDetached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
motorControl.setOnInputChangeHandler(mcInputChanged)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.setOnSensorUpdateHandler(mcSensorUpdated)
if serial_no != 0:
motorControl.openPhidget(serial_no)
else:
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
except PhidgetException as e:
rospy.logerr("Unable to register the handlers: %i: %s", e.code,
e.details)
return
except AttributeError as e:
rospy.logerr("Unable to register the handlers: %s", e)
return
except:
rospy.logerr("Unable to register the handlers: %s", sys.exc_info()[0])
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",
motorControl.getDeviceName(),
motorControl.getSerialNum(),
motorControl.getDeviceVersion())
# ensure the motor controller attempts to brake the linear
# when velocity is cut to 0
motorControl.setBraking(linear, 100)
# print motorControl.getBraking(linear)
minAcceleration = motorControl.getAccelerationMin(linear)
maxAcceleration = motorControl.getAccelerationMax(linear)
invert_speed = rospy.get_param('~invert_speed', False)
maxPos = rospy.get_param('~max_pos', 1014)
minPos = rospy.get_param('~min_pos', 10)
phidgetMotorTopic = rospy.Subscriber("PhidgetLinear", LinearCommand, move)
phidgetMotorService = rospy.Service('PhidgetLinear', Move, move)
posdataPub = rospy.Publisher("position", UInt32, latch=True)
rospy.spin()
def AttachMotorControl(databasepath, serialNumber):
def onAttachHandler(event):
logString = "MotorControl Attached " + str(event.device.getSerialNum())
#print(logString)
DisplayAttachedDeviceInfo(event.device)
def onDetachHandler(event):
logString = "MotorControl Detached " + str(event.device.getSerialNum())
#print(logString)
DisplayDetachedDeviceInfo(event.device)
event.device.closePhidget()
def onErrorHandler(event):
logString = "MotorControl Error " + str(event.device.getSerialNum()) + ", Error: " + event.description
print(logString)
DisplayErrorDeviceInfo(event.device)
def onServerConnectHandler(event):
logString = "MotorControl Server Connect " + str(event.device.getSerialNum())
#print(logString)
def onServerDisconnectHandler(event):
logString = "MotorControl Server Disconnect " + str(event.device.getSerialNum())
#print(logString)
def backEMFUpdateHandler(event):
logString = "MotorControl BackEMF Update"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_BACKEMFUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.voltage))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def currentChangeHandler(event):
logString = "MotorControl Current Change"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_CURRENTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.current))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def currentUpdateHandler(event):
logString = "MotorControl Current Update"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_CURRENTUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.current))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def inputChangeHandler(event):
logString = "MotorControl Input Change"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_INPUTCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.state))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionChangeHandler(event):
logString = "MotorControl Position Change"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_POSITIONCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.current))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def positionUpdateHandler(event):
logString = "MotorControl Position Update"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_POSITIONUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.position))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def sensorUpdateHandler(event):
logString = "MotorControl Sensor Update"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
#conn.execute("INSERT INTO MOTORCONTROL_SENSORUPDATE VALUES(NULL, DateTime('now'), ?, ?, ?)",
#(event.device.getSerialNum(), event.index, event.value))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
def velocityChangeHandler(event):
logString = "MotorControl Velocity Change"
#print(logString)
try:
conn = sqlite3.connect(databasepath)
conn.execute("INSERT INTO MOTORCONTROL_VELOCITYCHANGE VALUES(NULL, DateTime('now'), ?, ?, ?)",
(event.device.getSerialNum(), event.index, event.velocity))
conn.commit()
conn.close()
except sqlite3.Error as e:
print "An error occurred:", e.args[0]
try:
p = MotorControl()
p.setOnAttachHandler(onAttachHandler)
p.setOnDetachHandler(onDetachHandler)
p.setOnErrorhandler(onErrorHandler)
p.setOnServerConnectHandler(onServerConnectHandler)
p.setOnServerDisconnectHandler(onServerDisconnectHandler)
p.setOnBackEMFUpdateHandler (backEMFUpdateHandler)
p.setOnCurrentChangeHandler (currentChangeHandler)
p.setOnCurrentUpdateHandler (currentUpdateHandler)
p.setOnInputChangeHandler (inputChangeHandler)
p.setOnPositionChangeHandler(positionChangeHandler)
p.setOnPositionUpdateHandler(positionUpdateHandler)
p.setOnSensorUpdateHandler (sensorUpdateHandler)
p.setOnVelocityChangeHandler(velocityChangeHandler)
p.openPhidget(serialNumber)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting...")
exit(1)
