def setupMoveService():
"""Initialize the DriveMotors service
Establish a connection with the Phidget HC Motor Control and
then with the ROS Master as the service DriveMotors
"""
global motorControl, minAcceleration, maxAcceleration
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnDetachHandler(mcDetached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
motorControl.setOnInputChangeHandler(mcInputChanged)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
except:
rospy.logerr("Unable to register the handlers")
return
try:
motorControl.openPhidget()
except PhidgetException as e:
rospy.logerr("Fail to openPhidget() %i: %s", e.code, e.details)
return
try:
motorControl.waitForAttach(10000)
except PhidgetException as e:
rospy.logerr("Fail to attach to Phidget %i: %s", e.code, e.details)
return
if motorControl.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",
motorControl.getDeviceName(),
motorControl.getSerialNum(),
motorControl.getDeviceVersion())
minAcceleration = motorControl.getAccelerationMin(leftWheels)
maxAcceleration = motorControl.getAccelerationMax(leftWheels)
rospy.init_node('DriveMotors', log_level=rospy.DEBUG)
phidgetMotorService = rospy.Service('DriveMotors', Move, move)
rospy.spin()
class PhidgetMotorController:
def __init__(
self,
leftMotorId,
rightMotorId,
leftSignAdjust,
rightSignAdjust
):
self.leftWheels = leftMotorId
self.rightWheels = rightMotorId
self.defaultMotorSpeed = 100.0
self.motorMaxSpeed = 100
self.motorMinSpeed = 0
self.leftSignAdjust = leftSignAdjust
self.rightSignAdjust = rightSignAdjust
self.whichMotorFirst = self.rightWheels
self.motorControl = MotorControl()
self.motorControl.setOnAttachHandler(self.mcAttached)
self.motorControl.setOnDetachHandler(self.mcDetached)
self.motorControl.setOnErrorhandler(self.mcError)
self.motorControl.setOnCurrentChangeHandler(self.mcCurrentChanged)
self.motorControl.setOnInputChangeHandler(self.mcInputChanged)
self.motorControl.setOnVelocityChangeHandler(self.mcVelocityChanged)
try:
self.motorControl.openPhidget()
except PhidgetException, e:
print "openPhidget() failed"
print "code: %d" % e.code
print "message", e.message
raise
try:
self.motorControl.waitForAttach(10000)
except PhidgetException, e:
print "waitForAttach() failed"
print "code: %d" % e.code
print "message", e.message
raise
print("Motor Control %i: Input %i State: %s" % (source.getSerialNum(), e.index, e.state))
def motorControlVelocityChanged(e):
source = e.device
print("Motor Control %i: Motor %i Current Velocity: %f" % (source.getSerialNum(), e.index, e.velocity))
#Main Program Code
try:
#logging example, uncomment to generate a log file
#motorControl.enableLogging(PhidgetLogLevel.PHIDGET_LOG_VERBOSE, "phidgetlog.log")
motorControl.setOnAttachHandler(motorControlAttached)
motorControl.setOnDetachHandler(motorControlDetached)
motorControl.setOnErrorhandler(motorControlError)
motorControl.setOnCurrentChangeHandler(motorControlCurrentChanged)
motorControl.setOnInputChangeHandler(motorControlInputChanged)
motorControl.setOnVelocityChangeHandler(motorControlVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
motorControl.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
def setupMoveService():
"""Initialize the PhidgetMotor service
Establish a connection with the Phidget HC Motor Control and
then with the ROS Master as the service PhidgetMotor
"""
node_name = 'PhidgetMotor'
rospy.init_node(node_name, 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, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
timer = 0
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnDetachHandler(mcDetached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
motorControl.setOnInputChangeHandler(mcInputChanged)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
if serial_no != 0:
motorControl.openPhidget(serial_no)
else:
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is
separated of the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnDetachHandler(mcDetached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
motorControlRight.setOnInputChangeHandler(mcInputChanged)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.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())
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo("Device: %s, Serial: %d, Version: %d",
motorControlRight.getDeviceName(),
motorControlRight.getSerialNum(),
motorControlRight.getDeviceVersion())
minAcceleration = motorControl.getAccelerationMin(leftWheels)
maxAcceleration = motorControl.getAccelerationMax(leftWheels)
motors_inverted = rospy.get_param('~motors_inverted', False)
phidgetMotorTopic = rospy.Subscriber(node_name, MotorCommand, move)
phidgetMotorService = rospy.Service(node_name, Move, move)
if phidget1065 == True:
posdataPub = rospy.Publisher("position_data", PosMsg)
rospy.spin()
(source.getSerialNum(), e.index, e.state))
def motorControlVelocityChanged(e):
source = e.device
print("Motor Control %i: Motor %i Current Velocity: %f" %
(source.getSerialNum(), e.index, e.velocity))
#Main Program Code
try:
motorControl.setOnAttachHandler(motorControlAttached)
motorControl.setOnDetachHandler(motorControlDetached)
motorControl.setOnErrorhandler(motorControlError)
motorControl.setOnCurrentChangeHandler(motorControlCurrentChanged)
motorControl.setOnInputChangeHandler(motorControlInputChanged)
motorControl.setOnVelocityChangeHandler(motorControlVelocityChanged)
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
print("Opening phidget object....")
try:
motorControl.openPhidget()
except PhidgetException as e:
print("Phidget Exception %i: %s" % (e.code, e.details))
print("Exiting....")
exit(1)
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 setupMoveService():
"""Initialize the PhidgetMotor service
Establish a connection with the Phidget HC Motor Control and
then with the ROS Master as the service PhidgetMotor
"""
rospy.init_node(
'PhidgetMotor',
log_level = rospy.DEBUG
)
global motorControl, motorControlRight, minAcceleration, maxAcceleration, timer, motors_inverted, phidget1065, rightWheels, posdataPub
timer = 0
try:
motorControl = MotorControl()
except:
rospy.logerr("Unable to connect to Phidget HC Motor Control")
return
try:
motorControl.setOnAttachHandler(mcAttached)
motorControl.setOnDetachHandler(mcDetached)
motorControl.setOnErrorhandler(mcError)
motorControl.setOnCurrentChangeHandler(mcCurrentChanged)
motorControl.setOnInputChangeHandler(mcInputChanged)
motorControl.setOnVelocityChangeHandler(mcVelocityChanged)
motorControl.openPhidget()
#attach the board
motorControl.waitForAttach(10000)
"""use the function getMotorCount() to know how many motors the Phidget board can take
if the result is more than 1, we have a 1064 board and we take care of both motors with one motorControl variable. The corobot_phidgetIK handles the Phidget encoder board that is
separated of the phidget 1064.
if the result is equal to 1, we have two phidget 1065 boards. The one with serial number that is the lowest will be the left will, the other the right weel. The encoder has to be handled
in this file as it is part of the 1065 board.
"""
if motorControl.getMotorCount() == 1:
phidget1065 = True
rightWheels = 0
motorControlRight.setOnAttachHandler(mcAttached)
motorControlRight.setOnDetachHandler(mcDetached)
motorControlRight.setOnErrorhandler(mcError)
motorControlRight.setOnCurrentChangeHandler(mcCurrentChanged)
motorControlRight.setOnInputChangeHandler(mcInputChanged)
motorControlRight.setOnVelocityChangeHandler(mcVelocityChanged)
if motorControl.getSerialNum() > motorControlRight.getSerialNum():
""" As a rule, we need the serial number of the left board to be lower than for the right board. This is for consistancy for all the robots
"""
motorControlTemp = motorControl
motorControl = motorControlRight
motorControlRight = motorControlTemp
#Set up the encoders handler
motorControl.setOnPositionUpdateHandler(leftEncoderUpdated)
motorControlRight.setOnPositionUpdateHandler(rightEncoderUpdated)
#attach the board
motorControlRight.waitForAttach(10000)
except PhidgetException as e:
rospy.logerr("Unable to register the handlers: %i: %s", e.code, e.details)
return
except:
rospy.logerr("Unable to register the handlers")
return
if motorControl.isAttached():
rospy.loginfo(
"Device: %s, Serial: %d, Version: %d",
motorControl.getDeviceName(),
motorControl.getSerialNum(),
motorControl.getDeviceVersion()
)
if phidget1065 == True:
if motorControlRight.isAttached():
rospy.loginfo(
"Device: %s, Serial: %d, Version: %d",
motorControlRight.getDeviceName(),
motorControlRight.getSerialNum(),
motorControlRight.getDeviceVersion()
)
minAcceleration = motorControl.getAccelerationMin(leftWheels)
maxAcceleration = motorControl.getAccelerationMax(leftWheels)
motors_inverted = rospy.get_param('~motors_inverted', False)
phidgetMotorTopic = rospy.Subscriber("PhidgetMotor", MotorCommand ,move)
phidgetMotorService = rospy.Service('PhidgetMotor',Move, move)
if phidget1065 == True:
posdataPub = rospy.Publisher("position_data", PosMsg)
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)
